fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-12 04:51:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

PX4 ROS2 msg conformity and explicit topics

Browse Source 
- .msg files are PascalCase
 - topics are still either per msg or explicitly listed in TOPICS
 - compatible structs are still generated (eg struct msg_s), but ROS2 style px4::msg::Msg is also available
This commit is contained in:
Daniel Agar
2022-03-07 10:36:27 -05:00
parent 5affa693f2
commit 9ee8fa21cd
481 changed files with 5491 additions and 8399 deletions
Download Patch File Download Diff File Expand all files Collapse all files
msg/templates/px4/ros/msg.h.em
-94
View File
@@ -1,94 +0,0 @@
@###############################################
@#
@# PX4 ROS compatible message source code
@# generation for C++
@#
@# This file generates the multiplatform wrapper
@#
@# EmPy template for generating <msg>.h files
@# Based on the original template for ROS
@#
@###############################################
@# Start of Template
@#
@# Context:
@# - file_name_in (String) Source file
@# - spec (msggen.MsgSpec) Parsed specification of the .msg file
@###############################################
/****************************************************************************
*
* Copyright (C) 2013-2015 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.
*
****************************************************************************/
/* Auto-generated by genmsg_cpp from file @file_name_in */
@{
import genmsg.msgs
cpp_class = 'px4_%s'%spec.short_name
native_type = spec.short_name
topic_name = spec.short_name
}@
#pragma once
@##############################
@# Generic Includes
@##############################
#include "px4/@(topic_name).h"
#include "platforms/px4_message.h"
@##############################
@# Class
@##############################
namespace px4
{
class @(cpp_class) :
public PX4Message<@(native_type)>
{
public:
@(cpp_class)() :
PX4Message<@(native_type)>()
{}
@(cpp_class)(@(native_type) msg) :
PX4Message<@(native_type)>(msg)
{}
~@(cpp_class)() {}
static PX4TopicHandle handle() {return "@(topic_name)";}
};
};
msg/templates/px4/uorb/msg.h.em
-99
View File
@@ -1,99 +0,0 @@
@###############################################
@#
@# PX4 ROS compatible message source code
@# generation for C++
@#
@# This file generates the multiplatform wrapper
@#
@# EmPy template for generating <msg>.h files
@# Based on the original template for ROS
@#
@###############################################
@# Start of Template
@#
@# Context:
@# - file_name_in (String) Source file
@# - spec (msggen.MsgSpec) Parsed specification of the .msg file
@###############################################
/****************************************************************************
*
* Copyright (C) 2013-2015 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.
*
****************************************************************************/
/* Auto-generated by genmsg_cpp from file @file_name_in */
@{
import genmsg.msgs
native_type = '%s_s'%spec.short_name
topic_name = spec.short_name
}@
#pragma once
@##############################
@# Generic Includes
@##############################
#include <uORB/topics/@(topic_name).h>
#include "platforms/px4_message.h"
@##############################
@# Class
@##############################
namespace px4
{
@[for multi_topic in topics]@
@{
cpp_class = 'px4_%s'%multi_topic
}@
class __EXPORT @(cpp_class) :
public PX4Message<@(native_type)>
{
public:
@(cpp_class)() :
PX4Message<@(native_type)>()
{}
@(cpp_class)(@(native_type) msg) :
PX4Message<@(native_type)>(msg)
{}
~@(cpp_class)() {}
static PX4TopicHandle handle() {return ORB_ID(@(multi_topic));}
};
@[end for]
};
msg/templates/uorb/msg.cpp.em
-92
View File
@@ -1,92 +0,0 @@
@###############################################
@#
@# PX4 ROS compatible message source code
@# generation for C++
@#
@# EmPy template for generating <msg>.h files
@# Based on the original template for ROS
@#
@###############################################
@# Start of Template
@#
@# Context:
@# - file_name_in (String) Source file
@# - spec (msggen.MsgSpec) Parsed specification of the .msg file
@# - search_path (dict) search paths for genmsg
@# - topics (List of String) multi-topic names
@# - constrained_flash set to true if flash is constrained
@###############################################
/****************************************************************************
*
* Copyright (C) 2013-2021 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.
*
****************************************************************************/
/* Auto-generated by genmsg_cpp from file @file_name_in */
@{
import genmsg.msgs
from px_generate_uorb_topic_helper import * # this is in Tools/
uorb_struct = '%s_s'%spec.short_name
topic_name = spec.short_name
sorted_fields = sorted(spec.parsed_fields(), key=sizeof_field_type, reverse=True)
struct_size, padding_end_size = add_padding_bytes(sorted_fields, search_path)
topic_fields = ["%s %s" % (convert_type(field.type, True), field.name) for field in sorted_fields]
}@
#include <inttypes.h>
#include <px4_platform_common/log.h>
#include <px4_platform_common/defines.h>
#include <uORB/topics/@(topic_name).h>
#include <uORB/topics/uORBTopics.hpp>
#include <drivers/drv_hrt.h>
#include <lib/drivers/device/Device.hpp>
#include <lib/matrix/matrix/math.hpp>
#include <lib/mathlib/mathlib.h>
@# join all msg files in one line e.g: "float[3] position;float[3] velocity;bool armed"
@# This is used for the logger
constexpr char __orb_@(topic_name)_fields[] = "@( ";".join(topic_fields) );";
@[for multi_topic in topics]@
ORB_DEFINE(@multi_topic, struct @uorb_struct, @(struct_size-padding_end_size), __orb_@(topic_name)_fields, static_cast<uint8_t>(ORB_ID::@multi_topic));
@[end for]
void print_message(const orb_metadata *meta, const @uorb_struct& message)
{
if (sizeof(message) != meta->o_size) {
printf("unexpected message size for %s: %zu != %i\n", meta->o_name, sizeof(message), meta->o_size);
return;
}
orb_print_message_internal(meta, &message, true);
}
msg/templates/uorb/msg.h.em
+78 -9
View File
@@ -13,7 +13,7 @@
@# - file_name_in (String) Source file
@# - spec (msggen.MsgSpec) Parsed specification of the .msg file
@# - search_path (dict) search paths for genmsg
@# - topics (List of String) multi-topic names
@# - topics (List of String) topic names
@###############################################
/****************************************************************************
*
@@ -52,12 +52,17 @@
@{
import genmsg.msgs
import re
from px_generate_uorb_topic_helper import * # this is in Tools/
uorb_struct = '%s_s'%spec.short_name
uorb_struct_upper = spec.short_name.upper()
topic_name = spec.short_name
px4_struct = '%s'%name_snake_case
uorb_struct = '%s_s'%name_snake_case
uorb_struct_upper = name_snake_case.upper()
sorted_fields = sorted(spec.parsed_fields(), key=sizeof_field_type, reverse=True)
struct_size, padding_end_size = add_padding_bytes(sorted_fields, search_path)
topic_fields = ["%s %s" % (convert_type(field.type, True), field.name) for field in sorted_fields]
}@
#pragma once
@@ -67,6 +72,7 @@ topic_name = spec.short_name
@##############################
#include <uORB/uORB.h>
#include <stdio.h>
@##############################
@# Includes for dependencies
@@ -77,9 +83,16 @@ for field in spec.parsed_fields():
if (not field.is_header):
(package, name) = genmsg.names.package_resource_name(field.base_type)
package = package or spec.package # convert '' to package
name = re.sub(r'(?<!^)(?=[A-Z])', '_', name).lower()
print('#include <uORB/topics/%s.h>'%(name))
}@
#ifdef __PX4_ROS2
namespace px4_embedded {
#endif // __PX4_ROS2
@# Constants c style
#ifndef __cplusplus
@[for constant in spec.constants]@
@@ -124,14 +137,70 @@ for constant in spec.constants:
print('\tstatic constexpr %s %s = %s;'%(type_px4, constant.name, int(constant.val)))
}
static constexpr char FIELDS[] = "@( ";".join(topic_fields) );";
static constexpr size_t SIZE_NO_PADDING = @(struct_size-padding_end_size);
#endif
};
/* register this as object request broker structure */
@[for multi_topic in topics]@
ORB_DECLARE(@multi_topic);
@[end for]
#if defined(__PX4_ROS2)
} // namespace px4_embedded
#include "px4/msg/@(px4_struct).hpp"
using @(uorb_struct) = px4::msg::@(spec.short_name);
inline px4::msg::@(spec.short_name) px4_embedded_to_ros2(const px4_embedded::@(uorb_struct)& msg_in)
{
px4::msg::@(spec.short_name) msg_out;
@{
for field in spec.parsed_fields():
#print(field, field.is_builtin, field.base_type, field.is_array, field.array_len)
if field.is_builtin and not field.is_array:
print(" msg_out.%s = msg_in.%s;" % (field.name, field.name))
elif field.is_builtin and field.is_array:
print(" std::copy(std::begin(msg_in.%s), std::end(msg_in.%s), msg_out.%s.begin());" % (field.name, field.name, field.name))
elif not field.is_builtin:
#print(" msg_out.%s = px4_embedded_to_ros2(msg_in.%s);" % (field.name, field.name))
print(" // %s.%s not printed" % (name_snake_case, field.name))
else:
print(" #error %s.%s not printed" % (name_snake_case, field.name))
}@
return msg_out;
}
#else
# ifdef __cplusplus
namespace px4 {
namespace msg {
using @(spec.short_name) = @(uorb_struct);
} // namespace msg
} // namespace px4
namespace px4_embedded {
using @(uorb_struct) = @(uorb_struct);
}
# endif
#endif // PX4_ROS2
#ifdef __cplusplus
void print_message(const orb_metadata *meta, const @uorb_struct& message);
inline void print_message(const orb_metadata *meta, const @uorb_struct& message)
{
if (sizeof(message) != meta->o_size) {
printf("unexpected message size for %s: %zu != %i\n", meta->o_name, sizeof(message), meta->o_size);
return;
}
orb_print_message_internal(meta, &message, true);
}
#endif
msg/templates/uorb/uORBTopics.cpp.em
+15 -27
View File
@@ -1,18 +1,6 @@
@###############################################
@#
@# EmPy template for generating uORBTopics.cpp file
@# for logging purposes
@#
@###############################################
@# Start of Template
@#
@# Context:
@# - msgs (List) list of all msg files
@# - multi_topics (List) list of all multi-topic names
@###############################################
/****************************************************************************
*
* Copyright (C) 2013-2021 PX4 Development Team. All rights reserved.
* Copyright (C) 2021 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
@@ -43,23 +31,23 @@
*
****************************************************************************/
#include <uORB/topics/uORBTopics.hpp>
#include <uORB/uORB.h>
@{
msg_names = [mn.replace(".msg", "") for mn in msgs]
msgs_count = len(msg_names)
msg_names_all = list(set(msg_names + multi_topics)) # set() filters duplicates
msg_names_all.sort()
msgs_count_all = len(msg_names_all)
}@
@[for msg_name in msg_names]@
#include <uORB/topics/@(msg_name).h>
@[end for]
#include "uORBTopics.hpp"
// ORB_DEFINE(action_request, px4::msg::ActionRequest, px4_embedded::action_request_s::SIZE_NO_PADDING, px4_embedded::action_request_s::FIELDS, static_cast<uint8_t>(ORB_ID::action_request));
// ORB_DEFINE(actuator_armed, px4::msg::ActuatorArmed, px4_embedded::actuator_armed_s::SIZE_NO_PADDING, px4_embedded::actuator_armed_s::FIELDS, static_cast<uint8_t>(ORB_ID::actuator_armed));
//const constexpr struct orb_metadata *const uorb_topics_list[ORB_TOPICS_COUNT] = {
// ORB_ID(action_request),
// ORB_ID(actuator_armed),
@PX4_ORB_DEFINE_STR@
const constexpr struct orb_metadata *const uorb_topics_list[ORB_TOPICS_COUNT] = {
@[for idx, msg_name in enumerate(msg_names_all, 1)]@
ORB_ID(@(msg_name))@[if idx != msgs_count_all], @[end if]
@[end for]
@PX4_MSG_TOPIC_ORB_ID@
};
const struct orb_metadata *const *orb_get_topics()
msg/templates/uorb/uORBTopics.hpp.em
+34 -1
View File
@@ -57,14 +57,32 @@ msgs_count_all = len(msg_names_all)
#include <uORB/uORB.h>
static constexpr size_t ORB_TOPICS_COUNT{@(msgs_count_all)};
// TODO: ros2 friendly include paths
//#include <uORB/topics/action_request.h>
//#include <uORB/topics/actuator_armed.h>
@PX4_ORB_HEADER_INCLUDE_STR@
static constexpr size_t ORB_TOPICS_COUNT{@PX4_ORB_TOPIC_COUNT@};
static constexpr size_t orb_topics_count() { return ORB_TOPICS_COUNT; }
// ORB_DECLARE(action_request);
// ORB_DECLARE(actuator_armed);
@PX4_ORB_DECLARE_STR@
/*
* Returns array of topics metadata
*/
extern const struct orb_metadata *const *orb_get_topics() __EXPORT;
// enum class ORB_ID : uint8_t {
// action_request,
// actuator_armed,
enum class ORB_ID : uint8_t {
@[for idx, msg_name in enumerate(msg_names_all)]@
@(msg_name) = @(idx),
@@ -73,3 +91,18 @@ enum class ORB_ID : uint8_t {
};
const struct orb_metadata *get_orb_meta(ORB_ID id);
static constexpr const char *get_topic_string(ORB_ID orb_id)
{
switch (orb_id) {
//case ORB_ID::action_request: return "action_request";
//case ORB_ID::actuator_armed: return "actuator_armed";
@[for idx, msg_name in enumerate(msg_names_all)]@
case @(msg_name): return "@(msg_name)";
@[end for]
}
return nullptr;
}
msg/templates/uorb_microcdr/microRTPS_client.cpp.em
-306
View File
@@ -1,306 +0,0 @@
@###############################################
@#
@# EmPy template for generating microRTPS_client.cpp file
@#
@###############################################
@# Start of Template
@#
@# 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]
}@
/****************************************************************************
*
* Copyright (c) 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima).
* Copyright (c) 2018-2021 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 of the copyright holder 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 HOLDER 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 <microRTPS_transport.h>
#include <microRTPS_client.h>
#include <inttypes.h>
#include <cstdio>
#include <ctime>
#include <pthread.h>
#include <ucdr/microcdr.h>
#include <px4_time.h>
#include <uORB/uORB.h>
#include <uORB/PublicationMulti.hpp>
#include <uORB/Subscription.hpp>
@[for topic in list(set(topic_names))]@
#include <uORB/topics/@(topic).h>
#include <uORB_microcdr/topics/@(topic).h>
@[end for]@
using namespace time_literals;
@[if recv_topics]@
// 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]@
};
@[end if]@
@[if send_topics]@
// Subscribers for messages to send
struct SendTopicsSubs {
@[ for idx, topic in enumerate(send_topics)]@
uORB::Subscription @(topic)_sub{ORB_ID(@(topic))};
@[ end for]@
};
struct SendThreadArgs {
const uint32_t &datarate;
uint64_t &total_sent;
uint64_t &sent_last_sec;
uint64_t &sent;
int &sent_loop;
SendThreadArgs(const uint32_t &datarate_, uint64_t &total_sent_,
uint64_t &sent_last_sec_, uint64_t &sent_, int &sent_loop_)
: datarate(datarate_),
total_sent(total_sent_),
sent_last_sec(sent_last_sec_),
sent(sent_),
sent_loop(sent_loop_) {}
};
void *send(void *args)
{
char data_buffer[BUFFER_SIZE]{};
int read{0};
uint32_t length{0};
size_t header_length{0};
uint8_t last_msg_seq{0};
uint8_t last_remote_msg_seq{0};
struct SendThreadArgs *data = reinterpret_cast<struct SendThreadArgs *>(args);
SendTopicsSubs *subs = new SendTopicsSubs();
float bandwidth_mult{0};
float tx_interval{1.f};
uint64_t tx_last_sec_read{0};
hrt_abstime last_stats_update{0};
// ucdrBuffer to serialize using the user defined buffer
ucdrBuffer writer;
header_length = transport_node->get_header_length();
ucdr_init_buffer(&writer, reinterpret_cast<uint8_t *>(&data_buffer[header_length]), BUFFER_SIZE - header_length);
while (!_should_exit_task) {
@[ for idx, topic in enumerate(send_topics)]@
{
@(send_base_types[idx])_s @(topic)_data;
if (subs->@(topic)_sub.update(&@(topic)_data))
{
@[ if topic == 'Timesync' or topic == 'timesync']@
if (@(topic)_data.seq != last_remote_msg_seq && @(topic)_data.tc1 == 0) {
last_remote_msg_seq = @(topic)_data.seq;
@(topic)_data.timestamp = hrt_absolute_time();
@(topic)_data.seq = last_msg_seq;
@(topic)_data.tc1 = hrt_absolute_time() * 1000ULL;
@(topic)_data.ts1 = @(topic)_data.ts1;
last_msg_seq++;
@[ end if]@
// copy raw data into local buffer. Payload is shifted by header length to make room for header
serialize_@(send_base_types[idx])(&writer, &@(topic)_data, &data_buffer[header_length], &length);
if (0 < (read = transport_node->write(static_cast<char>(@(msgs[0].index(topic) + 1)), data_buffer, length))) {
data->total_sent += read;
tx_last_sec_read += read;
++data->sent;
}
@[ if topic == 'Timesync' or topic == 'timesync']@
}
@[ end if]@
}
}
@[ end for]@
if (hrt_absolute_time() - last_stats_update >= 1_s) {
data->sent_last_sec = tx_last_sec_read;
if (data->datarate > 0) {
bandwidth_mult = static_cast<float>(data->datarate) / static_cast<float>(tx_last_sec_read);
// Apply a low-pass filter to determine the new TX interval
tx_interval += 0.5f * (tx_interval / bandwidth_mult - tx_interval);
// Clamp the interval between 1 and 1000 ms
tx_interval = math::constrain(tx_interval, MIN_TX_INTERVAL_US, MAX_TX_INTERVAL_US);
}
tx_last_sec_read = 0;
last_stats_update = hrt_absolute_time();
}
px4_usleep(tx_interval);
++data->sent_loop;
}
delete(data);
delete(subs);
return nullptr;
}
static int launch_send_thread(pthread_t &sender_thread, struct SendThreadArgs &args)
{
pthread_attr_t sender_thread_attr;
pthread_attr_init(&sender_thread_attr);
pthread_attr_setstacksize(&sender_thread_attr, PX4_STACK_ADJUSTED(2250));
struct sched_param param;
(void)pthread_attr_getschedparam(&sender_thread_attr, &param);
param.sched_priority = SCHED_PRIORITY_DEFAULT;
(void)pthread_attr_setschedparam(&sender_thread_attr, &param);
int rc = pthread_create(&sender_thread, &sender_thread_attr, &send, (void *)&args);
if (rc != 0) {
errno = rc;
PX4_ERR("Could not create send thread (%d)", errno);
return -1;
}
rc = pthread_setname_np(sender_thread, "urtpsclient_snd");
if (pthread_setname_np(sender_thread, "urtpsclient_snd")) {
errno = rc;
PX4_ERR("Could not set pthread name for the send thread (%d)", errno);
}
pthread_attr_destroy(&sender_thread_attr);
return 0;
}
@[end if]@
void micrortps_start_topics(const uint32_t &datarate, struct timespec &begin, uint64_t &total_rcvd,
uint64_t &total_sent, uint64_t &sent_last_sec,
uint64_t &rcvd_last_sec, uint64_t &received, uint64_t &sent, int &rcvd_loop, int &sent_loop)
{
px4_clock_gettime(CLOCK_REALTIME, &begin);
_should_exit_task = false;
@[if recv_topics]@
char data_buffer[BUFFER_SIZE]{};
int read{0};
uint8_t topic_ID{255};
uint64_t rx_last_sec_read{0};
hrt_abstime last_stats_update{0};
RcvTopicsPubs *pubs = new RcvTopicsPubs();
// Set the main task name to 'urtpsclient_rcv' in case there is
// data to receive
px4_prctl(PR_SET_NAME, "urtpsclient_rcv", px4_getpid());
// ucdrBuffer to deserialize using the user defined buffer
ucdrBuffer reader;
ucdr_init_buffer(&reader, reinterpret_cast<uint8_t *>(data_buffer), BUFFER_SIZE);
@[end if]@
@[if send_topics]@
// var struct to be updated on the thread
SendThreadArgs *sender_thread_args = new SendThreadArgs(datarate, total_sent, sent_last_sec, sent, sent_loop);
// create a thread for sending data
pthread_t sender_thread;
launch_send_thread(sender_thread, (*sender_thread_args));
@[end if]@
while (!_should_exit_task) {
@[if recv_topics]@
while (0 < (read = transport_node->read(&topic_ID, data_buffer, BUFFER_SIZE))) {
total_rcvd += read;
rx_last_sec_read += read;
uint64_t read_time = hrt_absolute_time();
switch (topic_ID) {
@[ for idx, topic in enumerate(recv_topics)]@
case @(msgs[0].index(topic) + 1): {
@(receive_base_types[idx])_s @(topic)_data;
deserialize_@(receive_base_types[idx])(&reader, &@(topic)_data, data_buffer);
if (@(topic)_data.timestamp > read_time) {
// don't allow timestamps from the future
@(topic)_data.timestamp = read_time;
}
pubs->@(topic)_pub.publish(@(topic)_data);
++received;
}
break;
@[ end for]@
default:
PX4_WARN("Unexpected topic ID '%hhu' to getMsg. Please make sure the client is capable of parsing the message associated to the topic ID '%hhu'",
topic_ID, topic_ID);
break;
}
}
@[end if]@
if (hrt_absolute_time() - last_stats_update >= 1_s) {
rcvd_last_sec = rx_last_sec_read;
rx_last_sec_read = 0;
last_stats_update = hrt_absolute_time();
}
// loop forever if informed loop number is negative
if (_options.loops >= 0 && rcvd_loop >= _options.loops) { break; }
px4_usleep(_options.sleep_us);
++rcvd_loop;
}
@[if send_topics]@
_should_exit_task = true;
pthread_join(sender_thread, nullptr);
@[end if]@
@[if recv_topics]@
delete(pubs);
@[end if]@
}
msg/templates/uorb_microcdr/msg.cpp.em
-156
View File
@@ -1,156 +0,0 @@
@###############################################
@#
@# PX4 ROS compatible message source code
@# generation for C++
@#
@# EmPy template for generating <msg>.h files
@# Based on the original template for ROS
@#
@###############################################
@# Start of Template
@#
@# Context:
@# - file_name_in (String) Source file
@# - spec (msggen.MsgSpec) Parsed specification of the .msg file
@# - search_path (dict) search paths for genmsg
@###############################################
/****************************************************************************
*
* Copyright (C) 2013-2021 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.
*
****************************************************************************/
/* Auto-generated by genmsg_cpp from file @file_name_in */
@{
import genmsg.msgs
from px_generate_uorb_topic_helper import * # this is in Tools/
uorb_struct = '%s_s'%spec.short_name
topic_name = spec.short_name
}@
#include <px4_platform_common/px4_config.h>
#include <ucdr/microcdr.h>
#include <uORB/topics/@(topic_name).h>
#include <uORB_microcdr/topics/@(topic_name).h>
@#################################################
@# Searching for serialize function per each field
@#################################################
@{
def print_info(field):
print("type: ", field.type, "name: ", field.name, "base_type: ", \
field.base_type, "field.is_array:", ('0', '1')[field.is_array], " array_len: ", field.array_len, \
"is_builtin:", ('0', '1')[field.is_builtin], "is_header:", ('0', '1')[field.is_header])
def print_level_info(fields):
for field in fields:
print_info(field)
if (not field.is_builtin):
print("\n")
children_fields = get_children_fields(field.base_type, search_path)
print_level_info(children_fields)
print("\n")
def walk_through_parsed_fields():
print_level_info(spec.parsed_fields())
def get_serialization_type_name(type_name):
if type_name in type_serialize_map:
return type_serialize_map[type_name]
else:
raise Exception("Type {0} not supported, add to type_serialize_map!".format(type_name))
def add_serialize_functions(fields, scope_name):
for field in fields:
if (not field.is_header):
if (field.is_builtin):
if (not field.is_array):
print(" ucdr_serialize_" + str(get_serialization_type_name(field.type)) + "(writer, input->" + scope_name+str(field.name) + ");")
else:
print(" ucdr_serialize_array_" + str(get_serialization_type_name(field.base_type)) + "(writer, input->" + scope_name+str(field.name) + ", " + str(field.array_len) + ");")
else:
name = field.name
children_fields = get_children_fields(field.base_type, search_path)
if (scope_name): name = scope_name + name
if (not field.is_array):
add_serialize_functions(children_fields, name + '.')
else:
for i in range(field.array_len):
add_serialize_functions(children_fields, name + ('[%d].' %i))
def add_deserialize_functions(fields, scope_name):
for field in fields:
if (not field.is_header):
if (field.is_builtin):
if (not field.is_array):
print(" ucdr_deserialize_" + str(get_serialization_type_name(field.type)) + "(reader, &output->" + scope_name+str(field.name) + ");")
else:
print(" ucdr_deserialize_array_" + str(get_serialization_type_name(field.base_type)) + "(reader, output->" + scope_name+str(field.name) + ", " + str(field.array_len) + ");")
else:
name = field.name
children_fields = get_children_fields(field.base_type, search_path)
if (scope_name): name = scope_name + name
if (not field.is_array):
add_deserialize_functions(children_fields, name + '.')
else:
for i in range(field.array_len):
add_deserialize_functions(children_fields, name + ('[%d].' %i))
def add_code_to_serialize():
add_serialize_functions(spec.parsed_fields(), "")
def add_code_to_deserialize():
add_deserialize_functions(spec.parsed_fields(), "")
}@
void serialize_@(topic_name)(ucdrBuffer *writer, const struct @(uorb_struct) *input, char *output, uint32_t *length)
{
if (nullptr == writer || nullptr == input || nullptr == output || nullptr == length)
return;
ucdr_reset_buffer(writer);
@add_code_to_serialize()
(*length) = ucdr_buffer_length(writer);
}
void deserialize_@(topic_name)(ucdrBuffer *reader, struct @(uorb_struct) *output, const char *input)
{
if (nullptr == reader || nullptr == output || nullptr == input)
return;
ucdr_reset_buffer(reader);
@add_code_to_deserialize()
}
msg/templates/uorb_microcdr/msg.h.em
-69
View File
@@ -1,69 +0,0 @@
@###############################################
@#
@# PX4 ROS compatible message source code
@# generation for C++
@#
@# EmPy template for generating <msg>.h files
@# Based on the original template for ROS
@#
@###############################################
@# Start of Template
@#
@# Context:
@# - file_name_in (String) Source file
@# - spec (msggen.MsgSpec) Parsed specification of the .msg file
@###############################################
/****************************************************************************
*
* Copyright (C) 2013-2021 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.
*
****************************************************************************/
/* Auto-generated by genmsg_cpp from file @file_name_in */
@{
import genmsg.msgs
from px_generate_uorb_topic_helper import * # this is in Tools/
uorb_struct = '%s_s'%spec.short_name
topic_name = spec.short_name
}@
#pragma once
@##############################
@# Generic Includes
@##############################
#include <uORB/topics/@(topic_name).h>
#include <uORB_microcdr/topics/@(topic_name).h>
void serialize_@(topic_name)(ucdrBuffer *writer, const struct @(uorb_struct) *input, char *output, uint32_t *length);
void deserialize_@(topic_name)(ucdrBuffer *reader, struct @(uorb_struct) *output, const char *input);
msg/templates/uorb_microcdr/uORBTopics.cpp.em
-42
View File
@@ -1,42 +0,0 @@
@###############################################
@#
@# EmPy template for generating uORBTopics.cpp file
@# for logging purposes
@#
@###############################################
@# Start of Template
@###############################################
/****************************************************************************
*
* Copyright (C) 2013-2021 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 <uORB/uORB.h>
msg/templates/uorb_microcdr/uORBTopics.hpp.em
-44
View File
@@ -1,44 +0,0 @@
@###############################################
@#
@# EmPy template for generating uORBTopics.cpp file
@# for logging purposes
@#
@###############################################
@# Start of Template
@###############################################
/****************************************************************************
*
* Copyright (C) 2013-2021 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 <uORB/uORB.h>
msg/templates/urtps/Publisher.cpp.em
-219
View File
@@ -1,219 +0,0 @@
@###############################################
@#
@# EmPy template for generating <msg>_uRTPS_UART.cpp file
@#
@###############################################
@# Start of Template
@#
@# Context:
@# - fastrtps_version (str) FastRTPS version installed on the system
@# - ros2_distro (str) ROS2 distro name
@# - spec (msggen.MsgSpec) Parsed specification of the .msg file
@###############################################
@{
import genmsg.msgs
from packaging import version
import re
topic = alias if alias else spec.short_name
try:
ros2_distro = ros2_distro.decode("utf-8")
except AttributeError:
pass
topic_name = topic
# For ROS, use the topic pattern convention defined in
# http://wiki.ros.org/ROS/Patterns/Conventions
if ros2_distro:
topic_name_split = re.sub( r"([A-Z])", r" \1", topic).split()
topic_name = topic_name_split[0]
for w in topic_name_split[1:]:
topic_name += "_" + w
topic_name = topic_name.lower()
}@
/****************************************************************************
*
* Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima).
* Copyright (c) 2018-2021 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 of the copyright holder 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 HOLDER 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.
*
****************************************************************************/
/*!
* @@file @(topic)_Publisher.cpp
* This file contains the implementation of the publisher functions.
*
* This file was adapted from the fastrtpsgen tool.
*/
#include "@(topic)_Publisher.h"
#include <fastrtps/Domain.h>
#include <fastrtps/participant/Participant.h>
#include <fastrtps/attributes/ParticipantAttributes.h>
#include <fastrtps/publisher/Publisher.h>
#include <fastrtps/attributes/PublisherAttributes.h>
#include <fastrtps/transport/UDPv4TransportDescriptor.h>
@[if version.parse(fastrtps_version) >= version.parse('2.0')]@
#include <fastdds/rtps/transport/shared_mem/SharedMemTransportDescriptor.h>
using SharedMemTransportDescriptor = eprosima::fastdds::rtps::SharedMemTransportDescriptor;
@[end if]@
@(topic)_Publisher::@(topic)_Publisher()
: mp_participant(nullptr),
mp_publisher(nullptr)
{ }
@(topic)_Publisher::~@(topic)_Publisher()
{
Domain::removeParticipant(mp_participant);
}
bool @(topic)_Publisher::init(const std::string &ns, std::string topic_name)
{
// Create RTPSParticipant
ParticipantAttributes PParam;
@[if version.parse(fastrtps_version) < version.parse('2.0')]@
PParam.rtps.builtin.domainId = 0;
@[else]@
PParam.domainId = 0;
@[end if]@
@[if version.parse(fastrtps_version) <= version.parse('1.8.4')]@
PParam.rtps.builtin.leaseDuration = c_TimeInfinite;
@[else]@
PParam.rtps.builtin.discovery_config.leaseDuration = c_TimeInfinite;
@[end if]@
@[if ros2_distro]@
// ROS2 default memory management policy
PParam.rtps.builtin.writerHistoryMemoryPolicy = PREALLOCATED_WITH_REALLOC_MEMORY_MODE;
@[end if]@
std::string nodeName = ns;
nodeName.append("@(topic)_publisher");
PParam.rtps.setName(nodeName.c_str());
@[if ros2_distro]@
// Check if ROS_LOCALHOST_ONLY is set. This means that one wants to use only
// the localhost network for data sharing. If FastRTPS/DDS >= 2.0 and
// RMW_IMPLEMENTATION is FastDDS then the Shared Memory transport is used
const char* localhost_only = std::getenv("ROS_LOCALHOST_ONLY");
const char* rmw_implementation = std::getenv("RMW_IMPLEMENTATION");
const char* ros_distro = std::getenv("ROS_DISTRO");
if (localhost_only && strcmp(localhost_only, "1") == 0
&& ((rmw_implementation && ((strcmp(rmw_implementation, "rmw_fastrtps_cpp") == 0)
|| (strcmp(rmw_implementation, "rmw_fastrtps_dynamic_cpp") == 0)))
|| (!rmw_implementation && ros_distro && strcmp(ros_distro, "foxy") == 0))) {
// Create a custom network UDPv4 transport descriptor
// to whitelist the localhost
auto localhostUdpTransport = std::make_shared<UDPv4TransportDescriptor>();
localhostUdpTransport->interfaceWhiteList.emplace_back("127.0.0.1");
// Disable the built-in Transport Layer
PParam.rtps.useBuiltinTransports = false;
// Add the descriptor as a custom user transport
PParam.rtps.userTransports.push_back(localhostUdpTransport);
@[ if version.parse(fastrtps_version) >= version.parse('2.0')]@
// Add shared memory transport when available
auto shmTransport = std::make_shared<SharedMemTransportDescriptor>();
PParam.rtps.userTransports.push_back(shmTransport);
@[ end if]@
}
@[end if]@
mp_participant = Domain::createParticipant(PParam);
if (mp_participant == nullptr) {
return false;
}
// Register the type
Domain::registerType(mp_participant, static_cast<TopicDataType *>(&@(topic)DataType));
// Create Publisher
PublisherAttributes Wparam;
Wparam.topic.topicKind = NO_KEY;
Wparam.topic.topicDataType = @(topic)DataType.getName();
@[if ros2_distro]@
@[ if ros2_distro == "ardent"]@
Wparam.qos.m_partition.push_back("rt");
std::string topicName = ns;
@[ else]@
std::string topicName = "rt/";
topicName.append(ns);
@[ end if]@
// ROS2 default publish mode QoS policy
Wparam.qos.m_publishMode.kind = ASYNCHRONOUS_PUBLISH_MODE;
@[else]@
std::string topicName = ns;
@[end if]@
topic_name.empty() ? topicName.append("fmu/@(topic_name)/out") : topicName.append(topic_name);
Wparam.topic.topicName = topicName;
mp_publisher = Domain::createPublisher(mp_participant, Wparam, static_cast<PublisherListener *>(&m_listener));
if (mp_publisher == nullptr) {
return false;
}
return true;
}
void @(topic)_Publisher::PubListener::onPublicationMatched(Publisher *pub, MatchingInfo &info)
{
// The first 6 values of the ID guidPrefix of an entity in a DDS-RTPS Domain
// are the same for all its subcomponents (publishers, subscribers)
bool is_different_endpoint = false;
for (size_t i = 0; i < 6; i++) {
if (pub->getGuid().guidPrefix.value[i] != info.remoteEndpointGuid.guidPrefix.value[i]) {
is_different_endpoint = true;
break;
}
}
// If the matching happens for the same entity, do not make a match
if (is_different_endpoint) {
if (info.status == MATCHED_MATCHING) {
n_matched++;
std::cout << "\033[0;37m[ micrortps_agent ]\t@(topic) publisher matched\033[0m" << std::endl;
} else {
n_matched--;
std::cout << "\033[0;37m[ micrortps_agent ]\t@(topic) publisher unmatched\033[0m" << std::endl;
}
}
}
void @(topic)_Publisher::publish(@(topic)_msg_t *st)
{
mp_publisher->write(st);
}
msg/templates/urtps/Publisher.h.em
-120
View File
@@ -1,120 +0,0 @@
@###############################################
@#
@# EmPy template for generating <msg>_uRTPS_UART.cpp file
@#
@###############################################
@# Start of Template
@#
@# Context:
@# - ros2_distro (str) ROS2 distro name
@# - spec (msggen.MsgSpec) Parsed specification of the .msg file
@###############################################
@{
import genmsg.msgs
from packaging import version
import re
topic = alias if alias else spec.short_name
try:
ros2_distro = ros2_distro.decode("utf-8")
except AttributeError:
pass
}@
/****************************************************************************
*
* Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima).
* Copyright (c) 2018-2021 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 of the copyright holder 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 HOLDER 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.
*
****************************************************************************/
/*!
* @@file @(topic)_Publisher.h
* This header file contains the declaration of the publisher functions.
*
* This file was adapted from the fastrtpsgen tool.
*/
#ifndef _@(topic)__PUBLISHER_H_
#define _@(topic)__PUBLISHER_H_
#include <fastrtps/fastrtps_fwd.h>
#include <fastrtps/publisher/PublisherListener.h>
@[if version.parse(fastrtps_version) <= version.parse('1.7.2')]@
#include "@(topic)_PubSubTypes.h"
@[else]@
#include "@(topic)PubSubTypes.h"
@[end if]@
using namespace eprosima::fastrtps;
using namespace eprosima::fastrtps::rtps;
@[if version.parse(fastrtps_version) <= version.parse('1.7.2')]@
@[ if ros2_distro]@
using @(topic)_msg_t = @(package)::msg::dds_::@(topic)_;
using @(topic)_msg_datatype = @(package)::msg::dds_::@(topic)_PubSubType;
@[ else]@
using @(topic)_msg_t = @(topic)_;
using @(topic)_msg_datatype = @(topic)_PubSubType;
@[ end if]@
@[else]@
@[ if ros2_distro]@
using @(topic)_msg_t = @(package)::msg::@(topic);
using @(topic)_msg_datatype = @(package)::msg::@(topic)PubSubType;
@[ else]@
using @(topic)_msg_t = @(topic);
using @(topic)_msg_datatype = @(topic)PubSubType;
@[ end if]@
@[end if]@
class @(topic)_Publisher
{
public:
@(topic)_Publisher();
virtual ~@(topic)_Publisher();
bool init(const std::string &ns, std::string topic_name = "");
void run();
void publish(@(topic)_msg_t *st);
private:
Participant *mp_participant;
Publisher *mp_publisher;
class PubListener : public PublisherListener
{
public:
PubListener() : n_matched(0) {};
~PubListener() {};
void onPublicationMatched(Publisher *pub, MatchingInfo &info);
int n_matched;
} m_listener;
@(topic)_msg_datatype @(topic)DataType;
};
#endif // _@(topic)__PUBLISHER_H_
msg/templates/urtps/RtpsTopics.cpp.em
-187
View File
@@ -1,187 +0,0 @@
@###############################################
@#
@# EmPy template for generating RtpsTopics.cpp file
@#
@###############################################
@# Start of Template
@#
@# Context:
@# - spec (msggen.MsgSpec) Parsed specification of the .msg file
@###############################################
@{
import genmsg.msgs
import os
from px_generate_uorb_topic_files import MsgScope # this is in Tools/
send_topics = [(alias[idx] if alias[idx] else 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]
}@
/****************************************************************************
*
* Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima).
* Copyright (c) 2018-2021 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 of the copyright holder 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 HOLDER 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 "RtpsTopics.h"
bool RtpsTopics::init(std::condition_variable *t_send_queue_cv, std::mutex *t_send_queue_mutex,
std::queue<uint8_t> *t_send_queue, const std::string &ns)
{
@[if recv_topics]@
// Initialise subscribers
std::cout << "\033[0;36m--- Subscribers ---\033[0m" << std::endl;
@[for topic in recv_topics]@
if (_@(topic)_sub.init(@(msgs[0].index(topic) + 1), t_send_queue_cv, t_send_queue_mutex, t_send_queue, ns)) {
std::cout << "- @(topic) subscriber started" << std::endl;
} else {
std::cerr << "Failed starting @(topic) subscriber" << std::endl;
return false;
}
@[end for]@
std::cout << "\033[0;36m-----------------------\033[0m" << std::endl << std::endl;
@[end if]@
@[if send_topics]@
// Initialise publishers
std::cout << "\033[0;36m---- Publishers ----\033[0m" << std::endl;
@[for topic in send_topics]@
@[ if topic == 'Timesync' or topic == 'timesync']@
if (_@(topic)_pub.init(ns)) {
if (_@(topic)_fmu_in_pub.init(ns, std::string("fmu/timesync/in"))) {
_timesync->start(&_@(topic)_fmu_in_pub);
std::cout << "- @(topic) publishers started" << std::endl;
}
@[ elif topic == 'TimesyncStatus' or topic == 'timesync_status']@
if (_@(topic)_pub.init(ns, std::string("timesync_status"))) {
_timesync->init_status_pub(&_@(topic)_pub);
std::cout << "- @(topic) publisher started" << std::endl;
@[ else]@
if (_@(topic)_pub.init(ns)) {
std::cout << "- @(topic) publisher started" << std::endl;
@[ end if]@
} else {
std::cerr << "ERROR starting @(topic) publisher" << std::endl;
return false;
}
@[end for]@
std::cout << "\033[0;36m-----------------------\033[0m" << std::endl;
@[end if]@
return true;
}
@[if send_topics]@
template <typename T>
void RtpsTopics::sync_timestamp_of_incoming_data(T &msg) {
uint64_t timestamp = getMsgTimestamp(&msg);
uint64_t timestamp_sample = getMsgTimestampSample(&msg);
_timesync->subtractOffset(timestamp);
setMsgTimestamp(&msg, timestamp);
_timesync->subtractOffset(timestamp_sample);
setMsgTimestampSample(&msg, timestamp_sample);
}
void RtpsTopics::publish(const uint8_t topic_ID, char data_buffer[], size_t len)
{
switch (topic_ID) {
@[for topic in send_topics]@
case @(msgs[0].index(topic) + 1): { // @(topic) publisher
@(topic)_msg_t st;
eprosima::fastcdr::FastBuffer cdrbuffer(data_buffer, len);
eprosima::fastcdr::Cdr cdr_des(cdrbuffer);
st.deserialize(cdr_des);
@[ if topic == 'Timesync' or topic == 'timesync']@
_timesync->processTimesyncMsg(&st, &_@(topic)_pub);
@[ end if]@
// apply timestamp offset
sync_timestamp_of_incoming_data(st);
_@(topic)_pub.publish(&st);
}
break;
@[end for]@
default:
printf("\033[1;33m[ micrortps_agent ]\tUnexpected topic ID '%hhu' to publish. Please make sure the agent is capable of parsing the message associated to the topic ID '%hhu'\033[0m\n",
topic_ID, topic_ID);
break;
}
}
@[end if]@
@[if recv_topics]@
template <typename T>
void RtpsTopics::sync_timestamp_of_outgoing_data(T &msg) {
uint64_t timestamp = getMsgTimestamp(&msg);
uint64_t timestamp_sample = getMsgTimestampSample(&msg);
_timesync->addOffset(timestamp);
setMsgTimestamp(&msg, timestamp);
_timesync->addOffset(timestamp_sample);
setMsgTimestampSample(&msg, timestamp_sample);
}
bool RtpsTopics::getMsg(const uint8_t topic_ID, eprosima::fastcdr::Cdr &scdr)
{
bool ret = false;
switch (topic_ID) {
@[for topic in recv_topics]@
case @(msgs[0].index(topic) + 1): // @(topic) subscriber
if (_@(topic)_sub.hasMsg()) {
@(topic)_msg_t msg = _@(topic)_sub.getMsg();
// apply timestamp offset
sync_timestamp_of_outgoing_data(msg);
msg.serialize(scdr);
ret = true;
_@(topic)_sub.unlockMsg();
}
break;
@[end for]@
default:
printf("\033[1;33m[ micrortps_agent ]\tUnexpected topic ID '%hhu' to getMsg. Please make sure the agent is capable of parsing the message associated to the topic ID '%hhu'\033[0m\n",
topic_ID, topic_ID);
break;
}
return ret;
}
@[end if]@
msg/templates/urtps/RtpsTopics.h.em
-220
View File
@@ -1,220 +0,0 @@
@###############################################
@#
@# EmPy template for generating RtpsTopics.h file
@#
@###############################################
@# Start of Template
@#
@# Context:
@# - fastrtps_version (List[str]) FastRTPS version installed on the system
@# - package (List[str]) messages package name. Defaulted to 'px4'
@# - ros2_distro (List[str]) ROS2 distro name
@# - spec (msggen.MsgSpec) Parsed specification of the .msg file
@###############################################
@{
import genmsg.msgs
import os
from packaging import version
from px_generate_uorb_topic_files import MsgScope # this is in Tools/
send_topics = [(alias[idx] if alias[idx] else 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]
package = package[0]
fastrtps_version = fastrtps_version[0]
try:
ros2_distro = ros2_distro[0].decode("utf-8")
except AttributeError:
ros2_distro = ros2_distro[0]
}@
/****************************************************************************
*
* Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima).
* Copyright (c) 2018-2021 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 of the copyright holder 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 HOLDER 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 <fastcdr/Cdr.h>
#include <condition_variable>
#include <queue>
#include <type_traits>
#include "microRTPS_timesync.h"
@[for topic in send_topics]@
#include "@(topic)_Publisher.h"
@[end for]@
@[for topic in recv_topics]@
#include "@(topic)_Subscriber.h"
@[end for]@
@[for topic in (recv_topics + send_topics)]@
@[ if version.parse(fastrtps_version) <= version.parse('1.7.2')]@
@[ if ros2_distro]@
using @(topic)_msg_t = @(package)::msg::dds_::@(topic)_;
@[ else]@
using @(topic)_msg_t = @(topic)_;
@[ end if]@
@[ else]@
@[ if ros2_distro]@
using @(topic)_msg_t = @(package)::msg::@(topic);
@[ else]@
using @(topic)_msg_t = @(topic);
@[ end if]@
@[ end if]@
@[end for]@
class RtpsTopics
{
public:
bool init(std::condition_variable *t_send_queue_cv, std::mutex *t_send_queue_mutex, std::queue<uint8_t> *t_send_queue,
const std::string &ns);
void set_timesync(const std::shared_ptr<TimeSync> &timesync) { _timesync = timesync; };
@[if send_topics]@
template <typename T>
void sync_timestamp_of_incoming_data(T &msg);
void publish(const uint8_t topic_ID, char data_buffer[], size_t len);
@[end if]@
@[if recv_topics]@
template <typename T>
void sync_timestamp_of_outgoing_data(T &msg);
bool getMsg(const uint8_t topic_ID, eprosima::fastcdr::Cdr &scdr);
@[end if]@
private:
@[if send_topics]@
/** Publishers **/
@[for topic in send_topics]@
@[ if topic == 'Timesync' or topic == 'timesync']@
@(topic)_Publisher _@(topic)_pub;
@(topic)_Publisher _@(topic)_fmu_in_pub;
@[ else]@
@(topic)_Publisher _@(topic)_pub;
@[ end if]@
@[end for]@
@[end if]@
@[if recv_topics]@
/** Subscribers **/
@[for topic in recv_topics]@
@(topic)_Subscriber _@(topic)_sub;
@[end for]@
@[end if]@
// SFINAE
template<typename T> struct hasTimestampSample{
private:
template<typename U,
typename = decltype(std::declval<U>().timestamp_sample(int64_t()))>
static std::true_type detect(int);
template<typename U>
static std::false_type detect(...);
public:
static constexpr bool value = decltype(detect<T>(0))::value;
};
template<typename T>
inline typename std::enable_if < !hasTimestampSample<T>::value, uint64_t >::type
getMsgTimestampSample_impl(const T *) { return 0; }
/** Msg metada Getters **/
@[if version.parse(fastrtps_version) <= version.parse('1.7.2') or not ros2_distro]@
template <class T>
inline uint64_t getMsgTimestamp(const T *msg) { return msg->timestamp_(); }
template<typename T>
inline typename std::enable_if<hasTimestampSample<T>::value, uint64_t>::type
getMsgTimestampSample_impl(const T *msg) { return msg->timestamp_sample_(); }
template <class T>
inline uint8_t getMsgSysID(const T *msg) { return msg->sys_id_(); }
template <class T>
inline uint8_t getMsgSeq(const T *msg) { return msg->seq_(); }
@[elif ros2_distro]@
template <class T>
inline uint64_t getMsgTimestamp(const T *msg) { return msg->timestamp(); }
template<typename T>
inline typename std::enable_if<hasTimestampSample<T>::value, uint64_t>::type
getMsgTimestampSample_impl(const T *msg) { return msg->timestamp_sample(); }
template <class T>
inline uint8_t getMsgSysID(const T *msg) { return msg->sys_id(); }
template <class T>
inline uint8_t getMsgSeq(const T *msg) { return msg->seq(); }
@[end if]@
template <class T>
inline uint64_t getMsgTimestampSample(const T *msg) { return getMsgTimestampSample_impl(msg); }
template<typename T>
inline typename std::enable_if <!hasTimestampSample<T>::value, void>::type
setMsgTimestampSample_impl(T *, const uint64_t &) {}
/** Msg metadata Setters **/
@[if version.parse(fastrtps_version) <= version.parse('1.7.2') or not ros2_distro]@
template <class T>
inline void setMsgTimestamp(T *msg, const uint64_t &timestamp) { msg->timestamp_() = timestamp; }
template <class T>
inline typename std::enable_if<hasTimestampSample<T>::value, void>::type
setMsgTimestampSample_impl(T *msg, const uint64_t &timestamp_sample) { msg->timestamp_sample_() = timestamp_sample; }
template <class T>
inline void setMsgSysID(T *msg, const uint8_t &sys_id) { msg->sys_id_() = sys_id; }
template <class T>
inline void setMsgSeq(T *msg, const uint8_t &seq) { msg->seq_() = seq; }
@[elif ros2_distro]@
template <class T>
inline void setMsgTimestamp(T *msg, const uint64_t &timestamp) { msg->timestamp() = timestamp; }
template <class T>
inline typename std::enable_if<hasTimestampSample<T>::value, void>::type
setMsgTimestampSample_impl(T *msg, const uint64_t &timestamp_sample) { msg->timestamp_sample() = timestamp_sample; }
template <class T>
inline void setMsgSysID(T *msg, const uint8_t &sys_id) { msg->sys_id() = sys_id; }
template <class T>
inline void setMsgSeq(T *msg, const uint8_t &seq) { msg->seq() = seq; }
@[end if]@
template <class T>
inline void setMsgTimestampSample(T *msg, const uint64_t &timestamp_sample) { setMsgTimestampSample_impl(msg, timestamp_sample); }
/**
* @@brief Timesync object ptr.
* This object is used to compuyte and apply the time offsets to the
* messages timestamps.
*/
std::shared_ptr<TimeSync> _timesync;
};
msg/templates/urtps/Subscriber.cpp.em
-284
View File
@@ -1,284 +0,0 @@
@###############################################
@#
@# EmPy template for generating <msg>_uRTPS_UART.cpp file
@#
@###############################################
@# Start of Template
@#
@# Context:
@# - fastrtps_version (str) FastRTPS version installed on the system
@# - ros2_distro (str) ROS2 distro name
@# - spec (msggen.MsgSpec) Parsed specification of the .msg file
@###############################################
@{
import genmsg.msgs
from packaging import version
import re
topic = alias if alias else spec.short_name
try:
ros2_distro = ros2_distro.decode("utf-8")
except AttributeError:
pass
topic_name = topic
# For ROS, use the topic pattern convention defined in
# http://wiki.ros.org/ROS/Patterns/Conventions
if ros2_distro:
topic_name_split = re.sub( r"([A-Z])", r" \1", topic).split()
topic_name = topic_name_split[0]
for w in topic_name_split[1:]:
topic_name += "_" + w
topic_name = topic_name.lower()
}@
/****************************************************************************
*
* Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima).
* Copyright (c) 2018-2021 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 of the copyright holder 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 HOLDER 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.
*
****************************************************************************/
/*!
* @@file @(topic)_Subscriber.cpp
* This file contains the implementation of the subscriber functions.
*
* This file was adapted from the fastrtpsgen tool.
*/
#include "@(topic)_Subscriber.h"
#include <fastrtps/Domain.h>
#include <fastrtps/participant/Participant.h>
#include <fastrtps/attributes/ParticipantAttributes.h>
#include <fastrtps/subscriber/Subscriber.h>
#include <fastrtps/attributes/SubscriberAttributes.h>
#include <fastrtps/transport/UDPv4TransportDescriptor.h>
@[if version.parse(fastrtps_version) >= version.parse('2.0')]@
#include <fastdds/rtps/transport/shared_mem/SharedMemTransportDescriptor.h>
using SharedMemTransportDescriptor = eprosima::fastdds::rtps::SharedMemTransportDescriptor;
@[end if]@
@(topic)_Subscriber::@(topic)_Subscriber()
: mp_participant(nullptr),
mp_subscriber(nullptr)
{ }
@(topic)_Subscriber::~@(topic)_Subscriber()
{
Domain::removeParticipant(mp_participant);
}
bool @(topic)_Subscriber::init(uint8_t topic_ID, std::condition_variable *t_send_queue_cv,
std::mutex *t_send_queue_mutex, std::queue<uint8_t> *t_send_queue, const std::string &ns,
std::string topic_name)
{
m_listener.topic_ID = topic_ID;
m_listener.t_send_queue_cv = t_send_queue_cv;
m_listener.t_send_queue_mutex = t_send_queue_mutex;
m_listener.t_send_queue = t_send_queue;
// Create RTPSParticipant
ParticipantAttributes PParam;
@[if version.parse(fastrtps_version) < version.parse('2.0')]@
PParam.rtps.builtin.domainId = 0;
@[else]@
PParam.domainId = 0;
@[end if]@
@[if version.parse(fastrtps_version) <= version.parse('1.8.4')]@
PParam.rtps.builtin.leaseDuration = c_TimeInfinite;
@[else]@
PParam.rtps.builtin.discovery_config.leaseDuration = c_TimeInfinite;
@[end if]@
@[if ros2_distro]@
// ROS2 default memory management policy
PParam.rtps.builtin.writerHistoryMemoryPolicy = PREALLOCATED_WITH_REALLOC_MEMORY_MODE;
@[end if]@
std::string nodeName = ns;
nodeName.append("@(topic)_subscriber");
PParam.rtps.setName(nodeName.c_str());
@[if ros2_distro]@
// Check if ROS_LOCALHOST_ONLY is set. This means that one wants to use only
// the localhost network for data sharing. If FastRTPS/DDS >= 2.0 and
// RMW_IMPLEMENTATION is FastDDS then the Shared Memory transport is used
const char* localhost_only = std::getenv("ROS_LOCALHOST_ONLY");
const char* rmw_implementation = std::getenv("RMW_IMPLEMENTATION");
const char* ros_distro = std::getenv("ROS_DISTRO");
if (localhost_only && strcmp(localhost_only, "1") == 0
&& ((rmw_implementation && ((strcmp(rmw_implementation, "rmw_fastrtps_cpp") == 0)
|| (strcmp(rmw_implementation, "rmw_fastrtps_dynamic_cpp") == 0)))
|| (!rmw_implementation && ros_distro && strcmp(ros_distro, "foxy") == 0))) {
// Create a custom network UDPv4 transport descriptor
// to whitelist the localhost
auto localhostUdpTransport = std::make_shared<UDPv4TransportDescriptor>();
localhostUdpTransport->interfaceWhiteList.emplace_back("127.0.0.1");
// Disable the built-in Transport Layer
PParam.rtps.useBuiltinTransports = false;
// Add the descriptor as a custom user transport
PParam.rtps.userTransports.push_back(localhostUdpTransport);
@[ if version.parse(fastrtps_version) >= version.parse('2.0')]@
// Add shared memory transport when available
auto shmTransport = std::make_shared<SharedMemTransportDescriptor>();
PParam.rtps.userTransports.push_back(shmTransport);
@[ end if]@
}
@[end if]@
mp_participant = Domain::createParticipant(PParam);
if (mp_participant == nullptr) {
return false;
}
// Register the type
Domain::registerType(mp_participant, static_cast<TopicDataType *>(&@(topic)DataType));
// Create Subscriber
SubscriberAttributes Rparam;
Rparam.topic.topicKind = NO_KEY;
Rparam.topic.topicDataType = @(topic)DataType.getName();
@[if ros2_distro]@
@[ if ros2_distro == "ardent"]@
Rparam.qos.m_partition.push_back("rt");
std::string topicName = ns;
@[ else]@
std::string topicName = "rt/";
topicName.append(ns);
@[ end if]@
@[else]@
std::string topicName = ns;
@[end if]@
topic_name.empty() ? topicName.append("fmu/@(topic_name)/in") : topicName.append(topic_name);
Rparam.topic.topicName = topicName;
mp_subscriber = Domain::createSubscriber(mp_participant, Rparam, static_cast<SubscriberListener *>(&m_listener));
if (mp_subscriber == nullptr) {
return false;
}
return true;
}
void @(topic)_Subscriber::SubListener::onSubscriptionMatched(Subscriber *sub, MatchingInfo &info)
{
@# Since the time sync runs on the bridge itself, it is required that there is a
@# match between two topics of the same entity
@[if topic != 'Timesync' and topic != 'timesync' and topic != 'TimesyncStatus' and topic != 'timesync_status']@
// The first 6 values of the ID guidPrefix of an entity in a DDS-RTPS Domain
// are the same for all its subcomponents (publishers, subscribers)
bool is_different_endpoint = false;
for (size_t i = 0; i < 6; i++) {
if (sub->getGuid().guidPrefix.value[i] != info.remoteEndpointGuid.guidPrefix.value[i]) {
is_different_endpoint = true;
break;
}
}
// If the matching happens for the same entity, do not make a match
if (is_different_endpoint) {
if (info.status == MATCHED_MATCHING) {
n_matched++;
std::cout << "\033[0;37m[ micrortps_agent ]\t@(topic) subscriber matched\033[0m" << std::endl;
} else {
n_matched--;
std::cout << "\033[0;37m[ micrortps_agent ]\t@(topic) subscriber unmatched\033[0m" << std::endl;
}
}
@[else]@
(void)sub;
if (info.status == MATCHED_MATCHING) {
n_matched++;
} else {
n_matched--;
}
@[end if]@
}
void @(topic)_Subscriber::SubListener::onNewDataMessage(Subscriber *sub)
{
if (n_matched > 0) {
std::unique_lock<std::mutex> has_msg_lock(has_msg_mutex);
if (has_msg.load() == true) { // Check if msg has been fetched
has_msg_cv.wait(has_msg_lock); // Wait till msg has been fetched
}
has_msg_lock.unlock();
// Take data
if (sub->takeNextData(&msg, &m_info)) {
if (m_info.sampleKind == ALIVE) {
std::unique_lock<std::mutex> lk(*t_send_queue_mutex);
++n_msg;
has_msg = true;
t_send_queue->push(topic_ID);
lk.unlock();
t_send_queue_cv->notify_one();
}
}
}
}
bool @(topic)_Subscriber::hasMsg()
{
if (m_listener.n_matched > 0) {
return m_listener.has_msg.load();
}
return false;
}
@(topic)_msg_t @(topic)_Subscriber::getMsg()
{
return m_listener.msg;
}
void @(topic)_Subscriber::unlockMsg()
{
if (m_listener.n_matched > 0) {
std::unique_lock<std::mutex> has_msg_lock(m_listener.has_msg_mutex);
m_listener.has_msg = false;
has_msg_lock.unlock();
m_listener.has_msg_cv.notify_one();
}
}
msg/templates/urtps/Subscriber.h.em
-140
View File
@@ -1,140 +0,0 @@
@###############################################
@#
@# EmPy template for generating <msg>_uRTPS_UART.cpp file
@#
@###############################################
@# Start of Template
@#
@# Context:
@# - ros2_distro (str) ROS2 distro name
@# - spec (msggen.MsgSpec) Parsed specification of the .msg file
@###############################################
@{
import genmsg.msgs
from packaging import version
import re
topic = alias if alias else spec.short_name
try:
ros2_distro = ros2_distro.decode("utf-8")
except AttributeError:
pass
}@
/****************************************************************************
*
* Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima).
* Copyright (c) 2018-2021 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 of the copyright holder 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 HOLDER 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.
*
****************************************************************************/
/*!
* @@file @(topic)_Subscriber.h
* This header file contains the declaration of the subscriber functions.
*
* This file was adapted from the fastrtpsgen tool.
*/
#ifndef _@(topic)__SUBSCRIBER_H_
#define _@(topic)__SUBSCRIBER_H_
#include <fastrtps/fastrtps_fwd.h>
#include <fastrtps/subscriber/SubscriberListener.h>
#include <fastrtps/subscriber/SampleInfo.h>
@[if version.parse(fastrtps_version) <= version.parse('1.7.2')]@
#include "@(topic)_PubSubTypes.h"
@[else]@
#include "@(topic)PubSubTypes.h"
@[end if]@
#include <atomic>
#include <condition_variable>
#include <queue>
using namespace eprosima::fastrtps;
using namespace eprosima::fastrtps::rtps;
@[if version.parse(fastrtps_version) <= version.parse('1.7.2')]@
@[ if ros2_distro]@
using @(topic)_msg_t = @(package)::msg::dds_::@(topic)_;
using @(topic)_msg_datatype = @(package)::msg::dds_::@(topic)_PubSubType;
@[ else]@
using @(topic)_msg_t = @(topic)_;
using @(topic)_msg_datatype = @(topic)_PubSubType;
@[ end if]@
@[else]@
@[ if ros2_distro]@
using @(topic)_msg_t = @(package)::msg::@(topic);
using @(topic)_msg_datatype = @(package)::msg::@(topic)PubSubType;
@[ else]@
using @(topic)_msg_t = @(topic);
using @(topic)_msg_datatype = @(topic)PubSubType;
@[ end if]@
@[end if]@
class @(topic)_Subscriber
{
public:
@(topic)_Subscriber();
virtual ~@(topic)_Subscriber();
bool init(uint8_t topic_ID, std::condition_variable *t_send_queue_cv, std::mutex *t_send_queue_mutex,
std::queue<uint8_t> *t_send_queue, const std::string &ns, std::string topic_name = "");
void run();
bool hasMsg();
@(topic)_msg_t getMsg();
void unlockMsg();
private:
Participant *mp_participant;
Subscriber *mp_subscriber;
class SubListener : public SubscriberListener
{
public:
SubListener() : n_matched(0), n_msg(0), has_msg(false) {};
~SubListener() {};
void onSubscriptionMatched(Subscriber *sub, MatchingInfo &info);
void onNewDataMessage(Subscriber *sub);
SampleInfo_t m_info;
int n_matched;
int n_msg;
@(topic)_msg_t msg;
std::atomic_bool has_msg;
uint8_t topic_ID;
std::condition_variable *t_send_queue_cv;
std::mutex *t_send_queue_mutex;
std::queue<uint8_t> *t_send_queue;
std::condition_variable has_msg_cv;
std::mutex has_msg_mutex;
} m_listener;
@(topic)_msg_datatype @(topic)DataType;
};
#endif // _@(topic)__SUBSCRIBER_H_
msg/templates/urtps/microRTPS_agent.cpp.em
-404
View File
@@ -1,404 +0,0 @@
@###############################################
@#
@# EmPy template for generating microRTPS_agent.cpp file
@#
@###############################################
@# Start of Template
@#
@# Context:
@# - spec (msggen.MsgSpec) Parsed specification of the .msg file
@# - ros2_distro (List[str]) ROS2 distro name
@###############################################
@{
import genmsg.msgs
from px_generate_uorb_topic_files import MsgScope # this is in Tools/
try:
ros2_distro = ros2_distro[0].decode("utf-8")
except AttributeError:
ros2_distro = ros2_distro[0]
send_topics = [(alias[idx] if alias[idx] else 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]
}@
/****************************************************************************
*
* Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima).
* Copyright (c) 2018-2021 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 of the copyright holder 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 HOLDER 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 <thread>
#include <atomic>
#include <getopt.h>
#include <unistd.h>
#include <poll.h>
#include <chrono>
#include <ctime>
#include <csignal>
#include <termios.h>
#include <condition_variable>
#include <queue>
#include <fastcdr/Cdr.h>
#include <fastcdr/FastCdr.h>
#include <fastcdr/exceptions/Exception.h>
#include <fastrtps/Domain.h>
#include "microRTPS_transport.h"
#include "microRTPS_timesync.h"
#include "RtpsTopics.h"
@[if ros2_distro]@
#include <rclcpp/rclcpp.hpp>
@[end if]@
// Default values
#define SLEEP_US 1
#define MAX_SLEEP_US 1000000
#define BAUDRATE 460800
#define MAX_DATA_RATE 10000000
#define DEVICE "/dev/ttyACM0"
#define POLL_MS 1
#define MAX_POLL_MS 1000
#define DEFAULT_RECV_PORT 2020
#define DEFAULT_SEND_PORT 2019
#define DEFAULT_IP "127.0.0.1"
using namespace eprosima;
using namespace eprosima::fastrtps;
volatile sig_atomic_t running = 1;
std::unique_ptr<Transport_node> transport_node;
std::unique_ptr<RtpsTopics> topics;
uint32_t total_sent = 0, sent = 0;
struct options {
enum class eTransports {
UART,
UDP
};
eTransports transport = options::eTransports::UART;
char device[64] = DEVICE;
int sleep_us = SLEEP_US;
uint32_t baudrate = BAUDRATE;
int poll_ms = POLL_MS;
uint16_t recv_port = DEFAULT_RECV_PORT;
uint16_t send_port = DEFAULT_SEND_PORT;
char ip[16] = DEFAULT_IP;
bool sw_flow_control = false;
bool hw_flow_control = false;
bool verbose_debug = false;
std::string ns = "";
} _options;
static void usage(const char *name)
{
printf("usage: %s [options]\n\n"
" -b <baudrate> UART device baudrate. Defaults to 460800\n"
" -d <device> UART device. Defaults to /dev/ttyACM0\n"
" -f <sw-flow-control> Activates UART link SW flow control\n"
" -g <hw-flow-control> Activates UART link HW flow control\n"
" -i <ip-address> Target remote IP address for UDP. Defaults to 127.0.0.1\n"
" -n <namespace> Topics namespace. Identifies the vehicle in a multi-agent network\n"
" -o <poll-ms> UART polling timeout in milliseconds. Defaults to 1ms\n"
" -r <reception-port> UDP port for receiving (local). Defaults to 2020\n"
" -s <sending-port> UDP port for sending (remote). Defaults to 2019\n"
" -t <transport> [UART|UDP] Defaults to UART\n"
" -v <increase-verbosity> Add more verbosity\n"
" -w <sleep-time-us> Iteration time for data publishing to the DDS world, in microseconds.\n"
" Defaults to 1us\n"
" <ros-args> (ROS2 only) Allows to pass arguments to the timesync ROS2 node.\n"
" Currently used for setting the usage of simulation time by the node using\n"
" '--ros-args -p use_sim_time:=true'\n",
name);
}
static int parse_options(int argc, char **argv)
{
static const struct option options[] = {
{"baudrate", required_argument, NULL, 'b'},
{"device", required_argument, NULL, 'd'},
{"sw-flow-control", no_argument, NULL, 'f'},
{"hw-flow-control", no_argument, NULL, 'g'},
{"ip-address", required_argument, NULL, 'i'},
{"namespace", required_argument, NULL, 'n'},
{"poll-ms", required_argument, NULL, 'o'},
{"reception-port", required_argument, NULL, 'r'},
{"sending-port", required_argument, NULL, 's'},
{"transport", required_argument, NULL, 't'},
{"increase-verbosity", no_argument, NULL, 'v'},
{"sleep-time-us", required_argument, NULL, 'w'},
{"ros-args", required_argument, NULL, 0},
{"help", no_argument, NULL, 'h'},
{NULL, 0, NULL, 0}};
int ch;
while ((ch = getopt_long(argc, argv, "t:d:w:b:o:r:s:i:fghvn:", options, nullptr)) >= 0) {
switch (ch) {
case 't': _options.transport = strcmp(optarg, "UDP") == 0 ?
options::eTransports::UDP
: options::eTransports::UART; break;
case 'd': if (nullptr != optarg) strcpy(_options.device, optarg); break;
case 'w': _options.sleep_us = strtoul(optarg, nullptr, 10); break;
case 'b': _options.baudrate = strtoul(optarg, nullptr, 10); break;
case 'o': _options.poll_ms = strtol(optarg, nullptr, 10); break;
case 'r': _options.recv_port = strtoul(optarg, nullptr, 10); break;
case 's': _options.send_port = strtoul(optarg, nullptr, 10); break;
case 'i': if (nullptr != optarg) strcpy(_options.ip, optarg); break;
case 'f': _options.sw_flow_control = true; break;
case 'g': _options.hw_flow_control = true; break;
case 'h': usage(argv[0]); exit(0); break;
case 'v': _options.verbose_debug = true; break;
case 'n': if (nullptr != optarg) _options.ns = std::string(optarg) + "/"; break;
default:
@[if ros2_distro]@
break;
@[else]@
usage(argv[0]);
return -1;
@[end if]@
}
}
if (_options.poll_ms < POLL_MS) {
_options.poll_ms = POLL_MS;
printf("\033[1;33m[ micrortps_agent ]\tPoll timeout too low. Using %d ms instead\033[0m\n", POLL_MS);
} else if (_options.poll_ms > MAX_POLL_MS) {
_options.poll_ms = MAX_POLL_MS;
printf("\033[1;33m[ micrortps_agent ]\tPoll timeout too high. Using %d ms instead\033[0m\n", MAX_POLL_MS);
}
if (_options.sleep_us > MAX_SLEEP_US) {
_options.sleep_us = MAX_SLEEP_US;
printf("\033[1;33m[ micrortps_agent ]\tPublishing iteration cycle too slow. Using %d us instead\033[0m\n", MAX_SLEEP_US);
}
if (_options.hw_flow_control && _options.sw_flow_control) {
printf("\033[0;31m[ micrortps_agent ]\tHW and SW flow control set. Please set only one or another\033[0m\n");
return -1;
}
return 0;
}
@[if recv_topics]@
std::atomic<bool> exit_sender_thread(false);
std::condition_variable t_send_queue_cv;
std::mutex t_send_queue_mutex;
std::queue<uint8_t> t_send_queue;
void t_send(void *)
{
char data_buffer[BUFFER_SIZE] = {};
uint32_t length = 0;
uint8_t topic_ID = 255;
while (running && !exit_sender_thread) {
std::unique_lock<std::mutex> lk(t_send_queue_mutex);
while (t_send_queue.empty() && !exit_sender_thread) {
t_send_queue_cv.wait(lk);
}
topic_ID = t_send_queue.front();
t_send_queue.pop();
lk.unlock();
size_t header_length = transport_node->get_header_length();
/* make room for the header to fill in later */
eprosima::fastcdr::FastBuffer cdrbuffer(&data_buffer[header_length], sizeof(data_buffer) - header_length);
eprosima::fastcdr::Cdr scdr(cdrbuffer);
if (!exit_sender_thread) {
if (topics->getMsg(topic_ID, scdr)) {
length = scdr.getSerializedDataLength();
if (0 < (length = transport_node->write(topic_ID, data_buffer, length))) {
total_sent += length;
++sent;
}
}
}
}
}
@[end if]@
void signal_handler(int signum)
{
printf("\n\033[1;33m[ micrortps_agent ]\tInterrupt signal (%d) received.\033[0m\n", signum);
running = 0;
transport_node->close();
}
int main(int argc, char **argv)
{
printf("\033[0;37m--- MicroRTPS Agent ---\033[0m\n");
if (-1 == parse_options(argc, argv)) {
printf("\033[1;33m[ micrortps_agent ]\tEXITING...\033[0m\n");
return -1;
}
topics = std::make_unique<RtpsTopics>();
@[if ros2_distro]@
// Initialize communications via the rmw implementation and set up a global signal handler.
rclcpp::init(argc, argv, rclcpp::InitOptions());
@[end if]@
// register signal SIGINT and signal handler
signal(SIGINT, signal_handler);
printf("[ micrortps_agent ]\tStarting link...\n");
const char* localhost_only = std::getenv("ROS_LOCALHOST_ONLY");
if (localhost_only && strcmp(localhost_only, "1") == 0) {
printf("[ micrortps_agent ]\tUsing only the localhost network...\n");
}
/**
* Set the system ID to Mission Computer, in order to identify the agent side
*
* Note: theoretically a multi-agent system is possible, but this would require
* adjustments in the way the timesync is done (would have to create a timesync
* instance per agent). Keeping it contained for a 1:1 link for now is reasonable.
*/
const uint8_t sys_id = static_cast<uint8_t>(MicroRtps::System::MISSION_COMPUTER);
switch (_options.transport) {
case options::eTransports::UART: {
transport_node = std::make_unique<UART_node>(_options.device, _options.baudrate, _options.poll_ms,
_options.sw_flow_control, _options.hw_flow_control, sys_id, _options.verbose_debug);
printf("[ micrortps_agent ]\tUART transport: device: %s; baudrate: %d; poll: %dms; flow_control: %s\n",
_options.device, _options.baudrate, _options.poll_ms,
_options.sw_flow_control ? "SW enabled" : (_options.hw_flow_control ? "HW enabled" : "No"));
}
break;
case options::eTransports::UDP: {
transport_node = std::make_unique<UDP_node>(_options.ip, _options.recv_port, _options.send_port,
sys_id, _options.verbose_debug);
printf("[ micrortps_agent ]\tUDP transport: ip address: %s; recv port: %u; send port: %u\n",
_options.ip, _options.recv_port, _options.send_port);
}
break;
default:
printf("\033[0;37m[ micrortps_agent ]\tEXITING...\033[0m\n");
return -1;
}
if (0 > transport_node->init()) {
printf("\033[0;37m[ micrortps_agent ]\tEXITING...\033[0m\n");
return -1;
}
sleep(1);
@[if send_topics]@
char data_buffer[BUFFER_SIZE] = {};
int received = 0, loop = 0;
int length = 0, total_read = 0;
bool receiving = false;
uint8_t topic_ID = 255;
std::chrono::time_point<std::chrono::steady_clock> start, end;
@[end if]@
// Init timesync
topics->set_timesync(std::make_shared<TimeSync>(_options.verbose_debug));
@[if recv_topics]@
topics->init(&t_send_queue_cv, &t_send_queue_mutex, &t_send_queue, _options.ns);
@[end if]@
running = true;
@[if recv_topics]@
std::thread sender_thread(t_send, nullptr);
@[end if]@
while (running) {
@[if send_topics]@
++loop;
if (!receiving) { start = std::chrono::steady_clock::now(); }
// Publish messages received from UART
if (0 < (length = transport_node->read(&topic_ID, data_buffer, BUFFER_SIZE))) {
topics->publish(topic_ID, data_buffer, sizeof(data_buffer));
++received;
total_read += length;
receiving = true;
end = std::chrono::steady_clock::now();
}
@[else]@
usleep(_options.sleep_us);
@[end if]@
}
@[if recv_topics]@
exit_sender_thread = true;
t_send_queue_cv.notify_one();
sender_thread.join();
std::chrono::duration<double> elapsed_secs = end - start;
if (received > 0) {
printf("[ micrortps_agent ]\tRECEIVED: %d messages - %d bytes; %d LOOPS - %.03f seconds - %.02fKB/s\n",
received, total_read, loop, elapsed_secs.count(), static_cast<double>(total_read) / (1000 * elapsed_secs.count()));
}
@[end if]@
@[if recv_topics]@
if (sent > 0) {
printf("[ micrortps_agent ]\tSENT: %lu messages - %lu bytes\n", static_cast<unsigned long>(sent),
static_cast<unsigned long>(total_sent));
}
@[end if]@
@[if ros2_distro]@
rclcpp::shutdown();
@[end if]@
transport_node.reset();
topics.reset();
return 0;
}
msg/templates/urtps/microRTPS_agent_CMakeLists.txt.em
-63
View File
@@ -1,63 +0,0 @@
################################################################################
#
# Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima).
# Copyright (c) 2018-2021 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 of the copyright holder 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 HOLDER 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.
#
################################################################################
cmake_minimum_required(VERSION 2.8.12)
project(micrortps_agent)
# Find requirements
find_package(fastrtps REQUIRED)
find_package(fastcdr REQUIRED)
# Set C++14
include(CheckCXXCompilerFlag)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_COMPILER_IS_CLANG OR
CMAKE_CXX_COMPILER_ID MATCHES "Clang")
check_cxx_compiler_flag(--std=c++14 SUPPORTS_CXX14)
if(SUPPORTS_CXX14)
add_compile_options(--std=c++14)
else()
message(FATAL_ERROR "Compiler doesn't support C++14")
endif()
endif()
file(GLOB MICRORTPS_AGENT_SOURCES src/*.cpp src/*.h)
add_executable(micrortps_agent ${MICRORTPS_AGENT_SOURCES})
target_link_libraries(micrortps_agent fastrtps fastcdr)
# Install to '/usr/local/bin' if `make install` is used
install(
TARGETS micrortps_agent
ARCHIVE DESTINATION lib
RUNTIME DESTINATION bin
LIBRARY DESTINATION lib
)
msg/templates/urtps/microRTPS_timesync.cpp.em
-295
View File
@@ -1,295 +0,0 @@
@###############################################
@#
@# EmPy template for generating microRTPS_timesync.cpp file
@#
@###############################################
@# Start of Template
@#
@# Context:
@# - package (List[str]) messages package name. Defaulted to 'px4'
@# - ros2_distro (List[str]) ROS2 distro name
@###############################################
@{
import genmsg.msgs
from px_generate_uorb_topic_files import MsgScope # this is in Tools/
package = package[0]
fastrtps_version = fastrtps_version[0]
try:
ros2_distro = ros2_distro[0].decode("utf-8")
except AttributeError:
ros2_distro = ros2_distro[0]
}@
/****************************************************************************
*
* Copyright (c) 2020-2021 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 of the copyright holder 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 HOLDER 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.
*
****************************************************************************/
/*!
* @@file microRTPS_timesync.cpp
* @@brief source code for time sync implementation
*/
#include <time.h>
#include <cmath>
#include <iostream>
#include "microRTPS_timesync.h"
TimeSync::TimeSync(bool debug)
: _offset_ns(-1),
@[if ros2_distro]@
_timesync_node(std::make_shared<rclcpp::Node>("timesync_node")),
@[end if]@
_skew_ns_per_sync(0.0),
_num_samples(0),
_request_reset_counter(0),
_last_msg_seq(0),
_last_remote_msg_seq(0),
_debug(debug)
{ }
TimeSync::~TimeSync() { stop(); }
void TimeSync::start(TimesyncPublisher *pub)
{
stop();
@[if ros2_distro]@
auto spin_node = [this]() {
rclcpp::spin(_timesync_node);
};
@[end if]@
auto run = [this, pub]() {
while (!_request_stop) {
timesync_msg_t msg = newTimesyncMsg();
pub->publish(&msg);
std::this_thread::sleep_for(std::chrono::milliseconds(100));
}
};
_request_stop = false;
@[if ros2_distro]@
_timesync_node_thread.reset(new std::thread(spin_node));
@[end if]@
_send_timesync_thread.reset(new std::thread(run));
}
void TimeSync::init_status_pub(TimesyncStatusPublisher *status_pub)
{
auto run = [this, status_pub]() {
while (!_request_stop) {
timesync_status_msg_t status_msg = newTimesyncStatusMsg();
status_pub->publish(&status_msg);
std::this_thread::sleep_for(std::chrono::milliseconds(100));
}
};
_request_stop = false;
_send_timesync_status_thread.reset(new std::thread(run));
}
void TimeSync::stop()
{
_request_stop = true;
@[if ros2_distro]@
if (_timesync_node_thread && _timesync_node_thread->joinable()) { _timesync_node_thread->join(); }
@[end if]@
if (_send_timesync_thread && _send_timesync_thread->joinable()) { _send_timesync_thread->join(); }
if (_send_timesync_status_thread && _send_timesync_status_thread->joinable()) { _send_timesync_status_thread->join(); }
}
void TimeSync::reset()
{
_num_samples = 0;
_request_reset_counter = 0;
}
@[if ros2_distro]@
uint64_t TimeSync::getROSTimeNSec() const
{
return _timesync_node->now().nanoseconds();
}
uint64_t TimeSync::getROSTimeUSec() const
{
return RCL_NS_TO_US(getROSTimeNSec());
}
@[else]@
uint64_t TimeSync::getSteadyTimeNSec() const
{
auto time = std::chrono::steady_clock::now();
return std::chrono::time_point_cast<std::chrono::nanoseconds>(time).time_since_epoch().count();
}
uint64_t TimeSync::getSteadyTimeUSec() const
{
auto time = std::chrono::steady_clock::now();
return std::chrono::time_point_cast<std::chrono::microseconds>(time).time_since_epoch().count();
}
@[end if]@
bool TimeSync::addMeasurement(int64_t local_t1_ns, int64_t remote_t2_ns, int64_t local_t3_ns)
{
_rtti = local_t3_ns - local_t1_ns;
_remote_time_stamp = remote_t2_ns;
// assume rtti is evenly split both directions
int64_t remote_t3_ns = remote_t2_ns + _rtti.load() / 2ll;
int64_t measurement_offset = remote_t3_ns - local_t3_ns;
if (_request_reset_counter > REQUEST_RESET_COUNTER_THRESHOLD) {
reset();
if (_debug) { std::cout << "\033[1;33m[ micrortps__timesync ]\tTimesync clock changed, resetting\033[0m" << std::endl; }
}
if (_num_samples == 0) {
updateOffset(measurement_offset);
_skew_ns_per_sync = 0;
}
if (_num_samples >= WINDOW_SIZE) {
if (std::abs(measurement_offset - _offset_ns.load()) > TRIGGER_RESET_THRESHOLD_NS) {
_request_reset_counter++;
if (_debug) { std::cout << "\033[1;33m[ micrortps__timesync ]\tTimesync offset outlier, discarding\033[0m" << std::endl; }
return false;
} else {
_request_reset_counter = 0;
}
}
// ignore if rtti > 50ms
if (_rtti.load() > 50ll * 1000ll * 1000ll) {
if (_debug) { std::cout << "\033[1;33m[ micrortps__timesync ]\tRTTI too high for timesync: " << _rtti.load() / (1000ll * 1000ll) << "ms\033[0m" << std::endl; }
return false;
}
double alpha = ALPHA_FINAL;
double beta = BETA_FINAL;
if (_num_samples < WINDOW_SIZE) {
double schedule = (double)_num_samples / WINDOW_SIZE;
double s = 1. - exp(.5 * (1. - 1. / (1. - schedule)));
alpha = (1. - s) * ALPHA_INITIAL + s * ALPHA_FINAL;
beta = (1. - s) * BETA_INITIAL + s * BETA_FINAL;
}
_offset_prev = _offset_ns.load();
updateOffset(static_cast<int64_t>((_skew_ns_per_sync + _offset_ns.load()) * (1. - alpha) +
measurement_offset * alpha));
_skew_ns_per_sync =
static_cast<int64_t>(beta * (_offset_ns.load() - _offset_prev.load()) + (1. - beta) * _skew_ns_per_sync);
_num_samples++;
return true;
}
void TimeSync::processTimesyncMsg(timesync_msg_t *msg, TimesyncPublisher *pub)
{
if (getMsgSeq(msg) != _last_remote_msg_seq) {
_last_remote_msg_seq = getMsgSeq(msg);
if (getMsgTC1(msg) > 0) {
@[if ros2_distro]@
if (!addMeasurement(getMsgTS1(msg), getMsgTC1(msg), getROSTimeNSec())) {
@[else]@
if (!addMeasurement(getMsgTS1(msg), getMsgTC1(msg), getSteadyTimeNSec())) {
@[end if]@
if (_debug) { std::cerr << "\033[1;33m[ micrortps__timesync ]\tOffset not updated\033[0m" << std::endl; }
}
} else if (getMsgTC1(msg) == 0) {
@[if ros2_distro]@
setMsgTimestamp(msg, getROSTimeUSec());
@[else]@
setMsgTimestamp(msg, getSteadyTimeUSec());
@[end if]@
setMsgSeq(msg, getMsgSeq(msg) + 1);
@[if ros2_distro]@
setMsgTC1(msg, getROSTimeNSec());
@[else]@
setMsgTC1(msg, getSteadyTimeNSec());
@[end if]@
pub->publish(msg);
}
}
}
timesync_msg_t TimeSync::newTimesyncMsg()
{
timesync_msg_t msg{};
@[if ros2_distro]@
setMsgTimestamp(&msg, getROSTimeUSec());
@[else]@
setMsgTimestamp(&msg, getSteadyTimeUSec());
@[end if]@
setMsgSeq(&msg, _last_msg_seq);
setMsgTC1(&msg, 0);
@[if ros2_distro]@
setMsgTS1(&msg, getROSTimeNSec());
@[else]@
setMsgTS1(&msg, getSteadyTimeNSec());
@[end if]@
_last_msg_seq++;
return msg;
}
timesync_status_msg_t TimeSync::newTimesyncStatusMsg()
{
timesync_status_msg_t msg{};
@[if ros2_distro]@
setMsgTimestamp(&msg, getROSTimeUSec());
@[else]@
setMsgTimestamp(&msg, getSteadyTimeUSec());
@[end if]@
setMsgSourceProtocol(&msg, 1); // SOURCE_PROTOCOL_RTPS
setMsgRemoteTimeStamp(&msg, _remote_time_stamp.load() / 1000ULL);
setMsgObservedOffset(&msg, _offset_prev.load());
setMsgEstimatedOffset(&msg, _offset_ns.load());
setMsgRoundTripTime(&msg, _rtti.load() / 1000ll);
return msg;
}
msg/templates/urtps/microRTPS_timesync.h.em
-303
View File
@@ -1,303 +0,0 @@
@###############################################
@#
@# EmPy template for generating microRTPS_timesync.h file
@#
@###############################################
@# Start of Template
@#
@# Context:
@# - package (List[str]) messages package name. Defaulted to 'px4'
@# - ros2_distro (List[str]) ROS2 distro name
@###############################################
@{
import genmsg.msgs
from packaging import version
from px_generate_uorb_topic_files import MsgScope # this is in Tools/
package = package[0]
fastrtps_version = fastrtps_version[0]
try:
ros2_distro = ros2_distro[0].decode("utf-8")
except AttributeError:
ros2_distro = ros2_distro[0]
}@
/****************************************************************************
*
* Copyright (c) 2020-2021 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 of the copyright holder 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 HOLDER 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.
*
****************************************************************************/
/*!
* @@file microRTPS_timesync.h
* @@brief Adds time sync for the microRTPS bridge
* @@author Nuno Marques <nuno.marques@@dronesolutions.io>
* @@author Julian Kent <julian@@auterion.com>
*/
#pragma once
#include <atomic>
#include <functional>
#include <thread>
@[if ros2_distro]@
#include "Timesync_Publisher.h"
#include "TimesyncStatus_Publisher.h"
#include <rcl/time.h>
#include <rclcpp/clock.hpp>
#include <rclcpp/rclcpp.hpp>
@[else]@
#include "timesync_Publisher.h"
#include "timesync_status_Publisher.h"
@[end if]@
static constexpr double ALPHA_INITIAL = 0.05;
static constexpr double ALPHA_FINAL = 0.003;
static constexpr double BETA_INITIAL = 0.05;
static constexpr double BETA_FINAL = 0.003;
static constexpr int WINDOW_SIZE = 500;
static constexpr int64_t UNKNOWN = 0;
static constexpr int64_t TRIGGER_RESET_THRESHOLD_NS = 100ll * 1000ll * 1000ll;
static constexpr int REQUEST_RESET_COUNTER_THRESHOLD = 5;
@# Sets the timesync DDS type according to the FastRTPS and ROS2 version
@[if version.parse(fastrtps_version) <= version.parse('1.7.2')]@
@[ if ros2_distro]@
using timesync_msg_t = @(package)::msg::dds_::Timesync_;
using timesync_status_msg_t = @(package)::msg::dds_::TimesyncStatus_;
@[ else]@
using timesync_msg_t = timesync_;
using timesync_status_msg_t = timesync_status_;
@[ end if]@
@[else]@
@[ if ros2_distro]@
using timesync_msg_t = @(package)::msg::Timesync;
using timesync_status_msg_t = @(package)::msg::TimesyncStatus;
@[ else]@
using timesync_msg_t = timesync;
using timesync_status_msg_t = timesync_status;
@[ end if]@
@[end if]@
@# Sets the timesync publisher entity depending on using ROS2 or not
@[if ros2_distro]@
using TimesyncPublisher = Timesync_Publisher;
using TimesyncStatusPublisher = TimesyncStatus_Publisher;
@[else]@
using TimesyncPublisher = timesync_Publisher;
using TimesyncStatusPublisher = timesync_status_Publisher;
@[end if]@
class TimeSync
{
public:
TimeSync(bool debug);
virtual ~TimeSync();
/**
* @@brief Starts the timesync publishing thread
* @@param[in] pub The timesync publisher entity to use
*/
void start(TimesyncPublisher *pub);
/**
* @@brief Init and run the timesync status publisher thread
* @@param[in] pub The timesync status publisher entity to use
*/
void init_status_pub(TimesyncStatusPublisher *status_pub);
/**
* @@brief Resets the filter
*/
void reset();
/**
* @@brief Stops the timesync publishing thread
*/
void stop();
@[if ros2_distro]@
/**
* @@brief Get ROS time in nanoseconds. This will match the system time, which
* corresponds to the system-wide real time since epoch. If use_sim_time
* is set, the simulation time is grabbed by the node and used instead
* More info about ROS2 clock and time in:
* https://design.ros2.org/articles/clock_and_time.html
* @@return ROS time in nanoseconds
*/
uint64_t getROSTimeNSec() const;
/**
* @@brief Get ROS time in microseconds. Fetches the time from getROSTimeNSec()
* and converts it to microseconds
* @@return ROS time in microseconds
*/
uint64_t getROSTimeUSec() const;
@[else]@
/**
* @@brief Get clock monotonic time (raw) in nanoseconds
* @@return Steady CLOCK_MONOTONIC time in nanoseconds
*/
uint64_t getSteadyTimeNSec() const;
/**
* @@brief Get clock monotonic time (raw) in microseconds
* @@return Steady CLOCK_MONOTONIC time in microseconds
*/
uint64_t getSteadyTimeUSec() const;
@[end if]@
/**
* @@brief Adds a time offset measurement to be filtered
* @@param[in] local_t1_ns The agent CLOCK_MONOTONIC_RAW time in nanoseconds when the message was sent
* @@param[in] remote_t2_ns The (client) remote CLOCK_MONOTONIC time in nanoseconds
* @@param[in] local_t3_ns The agent current CLOCK_MONOTONIC time in nanoseconds
* @@return true or false depending if the time offset was updated
*/
bool addMeasurement(int64_t local_t1_ns, int64_t remote_t2_ns, int64_t local_t3_ns);
/**
* @@brief Processes DDS timesync message
* @@param[in,out] msg The timestamp msg to be processed
*/
void processTimesyncMsg(timesync_msg_t *msg, TimesyncPublisher *pub);
/**
* @@brief Creates a new timesync DDS message to be sent from the agent to the client
* @@return A new timesync message with the origin in the agent and with the agent timestamp
*/
timesync_msg_t newTimesyncMsg();
/**
* @@brief Creates a new timesync status DDS message to be sent from the agent to the client
* @@return A new timesync status message with the origin in the agent and with the agent timestamp
*/
timesync_status_msg_t newTimesyncStatusMsg();
/**
* @@brief Get the time sync offset in nanoseconds
* @@return The offset in nanoseconds
*/
inline int64_t getOffset() { return _offset_ns.load(); }
/**
* @@brief Sums the time sync offset to the timestamp
* @@param[in,out] timestamp The timestamp to add the offset to
*/
inline void addOffset(uint64_t &timestamp) { timestamp = (timestamp * 1000LL + _offset_ns.load()) / 1000ULL; }
/**
* @@brief Substracts the time sync offset to the timestamp
* @@param[in,out] timestamp The timestamp to subtract the offset of
*/
inline void subtractOffset(uint64_t &timestamp) { timestamp = (timestamp * 1000LL - _offset_ns.load()) / 1000ULL; }
private:
std::atomic<int64_t> _offset_ns;
std::atomic<int64_t> _offset_prev;
std::atomic<uint64_t> _remote_time_stamp;
std::atomic<uint32_t> _rtti;
@[if ros2_distro]@
/**
* @@brief A ROS2 node to fetch the ROS time to be used for timesync
*/
std::shared_ptr<rclcpp::Node> _timesync_node;
@[end if]@
int64_t _skew_ns_per_sync;
int64_t _num_samples;
int32_t _request_reset_counter;
uint8_t _last_msg_seq;
uint8_t _last_remote_msg_seq;
bool _debug;
std::unique_ptr<std::thread> _send_timesync_thread;
std::unique_ptr<std::thread> _send_timesync_status_thread;
@[if ros2_distro]@
std::unique_ptr<std::thread> _timesync_node_thread;
@[end if]@
std::atomic<bool> _request_stop{false};
/**
* @@brief Updates the offset of the time sync filter
* @@param[in] offset The value of the offset to update to
*/
inline void updateOffset(const uint64_t &offset) { _offset_ns.store(offset, std::memory_order_relaxed); }
/** Timesync msg Getters **/
@[if version.parse(fastrtps_version) <= version.parse('1.7.2') or not ros2_distro]@
inline uint64_t getMsgTimestamp(const timesync_msg_t *msg) { return msg->timestamp_(); }
inline uint8_t getMsgSeq(const timesync_msg_t *msg) { return msg->seq_(); }
inline int64_t getMsgTC1(const timesync_msg_t *msg) { return msg->tc1_(); }
inline int64_t getMsgTS1(const timesync_msg_t *msg) { return msg->ts1_(); }
@[elif ros2_distro]@
inline uint64_t getMsgTimestamp(const timesync_msg_t *msg) { return msg->timestamp(); }
inline uint8_t getMsgSeq(const timesync_msg_t *msg) { return msg->seq(); }
inline int64_t getMsgTC1(const timesync_msg_t *msg) { return msg->tc1(); }
inline int64_t getMsgTS1(const timesync_msg_t *msg) { return msg->ts1(); }
@[end if]@
/** Common timestamp setter **/
@[if version.parse(fastrtps_version) <= version.parse('1.7.2') or not ros2_distro]@
template <typename T>
inline void setMsgTimestamp(T *msg, const uint64_t &timestamp) { msg->timestamp_() = timestamp; }
@[elif ros2_distro]@
template <typename T>
inline void setMsgTimestamp(T *msg, const uint64_t &timestamp) { msg->timestamp() = timestamp; }
@[end if]@
/** Timesync msg Setters **/
@[if version.parse(fastrtps_version) <= version.parse('1.7.2') or not ros2_distro]@
inline void setMsgSeq(timesync_msg_t *msg, const uint8_t &seq) { msg->seq_() = seq; }
inline void setMsgTC1(timesync_msg_t *msg, const int64_t &tc1) { msg->tc1_() = tc1; }
inline void setMsgTS1(timesync_msg_t *msg, const int64_t &ts1) { msg->ts1_() = ts1; }
@[elif ros2_distro]@
inline void setMsgSeq(timesync_msg_t *msg, const uint8_t &seq) { msg->seq() = seq; }
inline void setMsgTC1(timesync_msg_t *msg, const int64_t &tc1) { msg->tc1() = tc1; }
inline void setMsgTS1(timesync_msg_t *msg, const int64_t &ts1) { msg->ts1() = ts1; }
@[end if]@
/** Timesync Status msg Setters **/
@[if version.parse(fastrtps_version) <= version.parse('1.7.2') or not ros2_distro]@
inline void setMsgSourceProtocol(timesync_status_msg_t *msg, const uint8_t &source_protocol) { msg->source_protocol_() = source_protocol; }
inline void setMsgRemoteTimeStamp(timesync_status_msg_t *msg, const uint64_t &remote_timestamp) { msg->remote_timestamp_() = remote_timestamp; }
inline void setMsgObservedOffset(timesync_status_msg_t *msg, const int64_t &observed_offset) { msg->observed_offset_() = observed_offset; }
inline void setMsgEstimatedOffset(timesync_status_msg_t *msg, const int64_t &estimated_offset) { msg->estimated_offset_() = estimated_offset; }
inline void setMsgRoundTripTime(timesync_status_msg_t *msg, const uint32_t &round_trip_time) { msg->round_trip_time_() = round_trip_time; }
@[elif ros2_distro]@
inline void setMsgSourceProtocol(timesync_status_msg_t *msg, const uint8_t &source_protocol) { msg->source_protocol() = source_protocol; }
inline void setMsgRemoteTimeStamp(timesync_status_msg_t *msg, const uint64_t &remote_timestamp) { msg->remote_timestamp() = remote_timestamp; }
inline void setMsgObservedOffset(timesync_status_msg_t *msg, const int64_t &observed_offset) { msg->observed_offset() = observed_offset; }
inline void setMsgEstimatedOffset(timesync_status_msg_t *msg, const int64_t &estimated_offset) { msg->estimated_offset() = estimated_offset; }
inline void setMsgRoundTripTime(timesync_status_msg_t *msg, const uint32_t &round_trip_time) { msg->round_trip_time() = round_trip_time; }
@[end if]@
};
msg/templates/urtps/microRTPS_transport.cpp
-801
View File
@@ -1,801 +0,0 @@
/****************************************************************************
*
* Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima).
* Copyright (c) 2018-2021 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 of the copyright holder 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 HOLDER 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 <unistd.h>
#include <fcntl.h>
#include <stdio.h>
#include <errno.h>
#include <sys/socket.h>
#include <cstdlib>
#include <inttypes.h>
#include <sys/ioctl.h>
#if __has_include("px4_platform_common/log.h") && __has_include("px4_platform_common/time.h")
#include <px4_platform_common/log.h>
#include <px4_platform_common/time.h>
#endif
#if defined(__linux__) || defined(__PX4_LINUX)
#include <linux/serial.h>
#endif /* __linux__ */
#include "microRTPS_transport.h"
/** CRC table for the CRC-16. The poly is 0x8005 (x^16 + x^15 + x^2 + 1) */
uint16_t const crc16_table[256] = {
0x0000, 0xC0C1, 0xC181, 0x0140, 0xC301, 0x03C0, 0x0280, 0xC241,
0xC601, 0x06C0, 0x0780, 0xC741, 0x0500, 0xC5C1, 0xC481, 0x0440,
0xCC01, 0x0CC0, 0x0D80, 0xCD41, 0x0F00, 0xCFC1, 0xCE81, 0x0E40,
0x0A00, 0xCAC1, 0xCB81, 0x0B40, 0xC901, 0x09C0, 0x0880, 0xC841,
0xD801, 0x18C0, 0x1980, 0xD941, 0x1B00, 0xDBC1, 0xDA81, 0x1A40,
0x1E00, 0xDEC1, 0xDF81, 0x1F40, 0xDD01, 0x1DC0, 0x1C80, 0xDC41,
0x1400, 0xD4C1, 0xD581, 0x1540, 0xD701, 0x17C0, 0x1680, 0xD641,
0xD201, 0x12C0, 0x1380, 0xD341, 0x1100, 0xD1C1, 0xD081, 0x1040,
0xF001, 0x30C0, 0x3180, 0xF141, 0x3300, 0xF3C1, 0xF281, 0x3240,
0x3600, 0xF6C1, 0xF781, 0x3740, 0xF501, 0x35C0, 0x3480, 0xF441,
0x3C00, 0xFCC1, 0xFD81, 0x3D40, 0xFF01, 0x3FC0, 0x3E80, 0xFE41,
0xFA01, 0x3AC0, 0x3B80, 0xFB41, 0x3900, 0xF9C1, 0xF881, 0x3840,
0x2800, 0xE8C1, 0xE981, 0x2940, 0xEB01, 0x2BC0, 0x2A80, 0xEA41,
0xEE01, 0x2EC0, 0x2F80, 0xEF41, 0x2D00, 0xEDC1, 0xEC81, 0x2C40,
0xE401, 0x24C0, 0x2580, 0xE541, 0x2700, 0xE7C1, 0xE681, 0x2640,
0x2200, 0xE2C1, 0xE381, 0x2340, 0xE101, 0x21C0, 0x2080, 0xE041,
0xA001, 0x60C0, 0x6180, 0xA141, 0x6300, 0xA3C1, 0xA281, 0x6240,
0x6600, 0xA6C1, 0xA781, 0x6740, 0xA501, 0x65C0, 0x6480, 0xA441,
0x6C00, 0xACC1, 0xAD81, 0x6D40, 0xAF01, 0x6FC0, 0x6E80, 0xAE41,
0xAA01, 0x6AC0, 0x6B80, 0xAB41, 0x6900, 0xA9C1, 0xA881, 0x6840,
0x7800, 0xB8C1, 0xB981, 0x7940, 0xBB01, 0x7BC0, 0x7A80, 0xBA41,
0xBE01, 0x7EC0, 0x7F80, 0xBF41, 0x7D00, 0xBDC1, 0xBC81, 0x7C40,
0xB401, 0x74C0, 0x7580, 0xB541, 0x7700, 0xB7C1, 0xB681, 0x7640,
0x7200, 0xB2C1, 0xB381, 0x7340, 0xB101, 0x71C0, 0x7080, 0xB041,
0x5000, 0x90C1, 0x9181, 0x5140, 0x9301, 0x53C0, 0x5280, 0x9241,
0x9601, 0x56C0, 0x5780, 0x9741, 0x5500, 0x95C1, 0x9481, 0x5440,
0x9C01, 0x5CC0, 0x5D80, 0x9D41, 0x5F00, 0x9FC1, 0x9E81, 0x5E40,
0x5A00, 0x9AC1, 0x9B81, 0x5B40, 0x9901, 0x59C0, 0x5880, 0x9841,
0x8801, 0x48C0, 0x4980, 0x8941, 0x4B00, 0x8BC1, 0x8A81, 0x4A40,
0x4E00, 0x8EC1, 0x8F81, 0x4F40, 0x8D01, 0x4DC0, 0x4C80, 0x8C41,
0x4400, 0x84C1, 0x8581, 0x4540, 0x8701, 0x47C0, 0x4680, 0x8641,
0x8201, 0x42C0, 0x4380, 0x8341, 0x4100, 0x81C1, 0x8081, 0x4040
};
Transport_node::Transport_node(const uint8_t sys_id, const bool debug):
_rx_buff_pos(0),
_debug(debug),
_sys_id(sys_id)
{
}
Transport_node::~Transport_node()
{
}
uint16_t Transport_node::crc16_byte(uint16_t crc, const uint8_t data)
{
return (crc >> 8) ^ crc16_table[(crc ^ data) & 0xff];
}
uint16_t Transport_node::crc16(uint8_t const *buffer, size_t len)
{
uint16_t crc = 0;
while (len--) {
crc = crc16_byte(crc, *buffer++);
}
return crc;
}
ssize_t Transport_node::read(uint8_t *topic_id, char out_buffer[], size_t buffer_len)
{
if (nullptr == out_buffer || nullptr == topic_id || !fds_OK()) {
return -1;
}
*topic_id = 255;
ssize_t len = node_read((void *)(_rx_buffer + _rx_buff_pos), sizeof(_rx_buffer) - _rx_buff_pos);
if (len < 0) {
int errsv = errno;
if (errsv && EAGAIN != errsv && ETIMEDOUT != errsv) {
#ifndef PX4_DEBUG
if (_debug) { printf("\033[0;31m[ micrortps_transport ]\tRead fail %d\033[0m\n", errsv); }
#else
if (_debug) { PX4_DEBUG("Read fail %d", errsv); }
#endif /* PX4_DEBUG */
}
return len;
}
_rx_buff_pos += len;
// We read some
size_t header_size = sizeof(struct Header);
// but not enough
if (_rx_buff_pos < header_size) {
return 0;
}
uint32_t msg_start_pos = 0;
for (msg_start_pos = 0; msg_start_pos <= _rx_buff_pos - header_size; ++msg_start_pos) {
if ('>' == _rx_buffer[msg_start_pos] && memcmp(_rx_buffer + msg_start_pos, ">>>", 3) == 0) {
break;
}
}
// Start not found
if (msg_start_pos > (_rx_buff_pos - header_size)) {
#ifndef PX4_DEBUG
if (_debug) { printf("\033[1;33m[ micrortps_transport ]\t (↓↓ %" PRIu32 ")\033[0m\n", msg_start_pos); }
#else
if (_debug) { PX4_DEBUG(" (↓↓ %" PRIu32 ")", msg_start_pos); }
#endif /* PX4_DEBUG */
// All we've checked so far is garbage, drop it - but save unchecked bytes
memmove(_rx_buffer, _rx_buffer + msg_start_pos, _rx_buff_pos - msg_start_pos);
_rx_buff_pos -= msg_start_pos;
return -1;
}
// [>,>,>,topic_id,sys_id,seq,payload_length_H,payload_length_L,CRCHigh,CRCLow,payloadStart, ... ,payloadEnd]
struct Header *header = (struct Header *)&_rx_buffer[msg_start_pos];
uint32_t payload_len = ((uint32_t)header->payload_len_h << 8) | header->payload_len_l;
// The received message comes from this system. Discard it.
// This might happen when:
// 1. The same UDP port is being used to send a rcv packets or
// 2. The same topic on the agent is being used for outgoing and incoming data
if (header->sys_id == _sys_id) {
// Drop the message and continue with the read buffer
memmove(_rx_buffer, _rx_buffer + msg_start_pos + 1, _rx_buff_pos - (msg_start_pos + 1));
_rx_buff_pos -= (msg_start_pos + 1);
return -1;
}
// The message won't fit the buffer.
if (buffer_len < header_size + payload_len) {
// Drop the message and continue with the read buffer
memmove(_rx_buffer, _rx_buffer + msg_start_pos + 1, _rx_buff_pos - (msg_start_pos + 1));
_rx_buff_pos -= (msg_start_pos + 1);
return -EMSGSIZE;
}
// We do not have a complete message yet
if (msg_start_pos + header_size + payload_len > _rx_buff_pos) {
// If there's garbage at the beginning, drop it
if (msg_start_pos > 0) {
#ifndef PX4_DEBUG
if (_debug) { printf("\033[1;33m[ micrortps_transport ]\t (↓ %" PRIu32 ")\033[0m\n", msg_start_pos); }
#else
if (_debug) { PX4_DEBUG(" (↓ %" PRIu32 ")", msg_start_pos); }
#endif /* PX4_DEBUG */
memmove(_rx_buffer, _rx_buffer + msg_start_pos, _rx_buff_pos - msg_start_pos);
_rx_buff_pos -= msg_start_pos;
}
return 0;
}
uint16_t read_crc = ((uint16_t)header->crc_h << 8) | header->crc_l;
uint16_t calc_crc = crc16((uint8_t *)_rx_buffer + msg_start_pos + header_size, payload_len);
if (read_crc != calc_crc) {
#ifndef PX4_DEBUG
if (_debug) { printf("\033[0;31m[ micrortps_transport ]\tBad CRC %" PRIu16 " != %" PRIu16 "\t\t(↓ %lu)\033[0m\n", read_crc, calc_crc, (unsigned long)(header_size + payload_len)); }
#else
if (_debug) { PX4_DEBUG("Bad CRC %u != %u\t\t(↓ %lu)", read_crc, calc_crc, (unsigned long)(header_size + payload_len)); }
#endif /* PX4_DEBUG */
// Drop garbage up just beyond the start of the message
memmove(_rx_buffer, _rx_buffer + (msg_start_pos + 1), _rx_buff_pos);
// If there is a CRC error, the payload len cannot be trusted
_rx_buff_pos -= (msg_start_pos + 1);
len = -1;
} else {
// copy message to outbuffer and set other return values
memmove(out_buffer, _rx_buffer + msg_start_pos + header_size, payload_len);
*topic_id = header->topic_id;
len = payload_len + header_size;
// discard message from _rx_buffer
_rx_buff_pos -= msg_start_pos + header_size + payload_len;
memmove(_rx_buffer, _rx_buffer + msg_start_pos + header_size + payload_len, _rx_buff_pos);
}
return len;
}
size_t Transport_node::get_header_length()
{
return sizeof(struct Header);
}
ssize_t Transport_node::write(const uint8_t topic_id, char buffer[], size_t length)
{
if (!fds_OK()) {
return -1;
}
static struct Header header = {{'>', '>', '>'}, 0u, 0u, 0u, 0u, 0u, 0u, 0u};
// [>,>,>,topic_id,seq,payload_length,CRCHigh,CRCLow,payload_start, ... ,payload_end]
uint16_t crc = crc16((uint8_t *)&buffer[sizeof(header)], length);
header.topic_id = topic_id;
header.sys_id = _sys_id;
header.seq = _seq_number++;
header.payload_len_h = (length >> 8) & 0xff;
header.payload_len_l = length & 0xff;
header.crc_h = (crc >> 8) & 0xff;
header.crc_l = crc & 0xff;
/* Headroom for header is created in client */
/* Fill in the header in the same payload buffer to call a single node_write */
memcpy(buffer, &header, sizeof(header));
ssize_t len = node_write(buffer, length + sizeof(header));
if (len != ssize_t(length + sizeof(header))) {
return len;
}
return len + sizeof(header);
}
UART_node::UART_node(const char *uart_name, const uint32_t baudrate,
const uint32_t poll_ms, const bool hw_flow_control,
const bool sw_flow_control, const uint8_t sys_id,
const bool debug):
Transport_node(sys_id, debug),
_uart_fd(-1),
_baudrate(baudrate),
_poll_ms(poll_ms),
_hw_flow_control(hw_flow_control),
_sw_flow_control(sw_flow_control)
{
if (nullptr != uart_name) {
strcpy(_uart_name, uart_name);
}
}
UART_node::~UART_node()
{
close();
}
int UART_node::init()
{
// Open a serial port
_uart_fd = open(_uart_name, O_RDWR | O_NOCTTY | O_NONBLOCK);
if (_uart_fd < 0) {
#ifndef PX4_ERR
printf("\033[0;31m[ micrortps_transport ]\tUART transport: Failed to open device: %s (%d)\033[0m\n", _uart_name, errno);
#else
PX4_ERR("UART transport: Failed to open device: %s (%d)", _uart_name, errno);
#endif /* PX4_ERR */
return -errno;
}
// If using shared UART, no need to set it up
if (_baudrate == 0) {
_poll_fd[0].fd = _uart_fd;
_poll_fd[0].events = POLLIN;
return _uart_fd;
}
// Try to set baud rate
struct termios uart_config;
int termios_state;
// Back up the original uart configuration to restore it after exit
if ((termios_state = tcgetattr(_uart_fd, &uart_config)) < 0) {
int errno_bkp = errno;
#ifndef PX4_ERR
printf("\033[0;31m[ micrortps_transport ]\tUART transport: ERR GET CONF %s: %d (%d)\n\033[0m", _uart_name, termios_state,
errno);
#else
PX4_ERR("UART transport: ERR GET CONF %s: %d (%d)", _uart_name, termios_state, errno);
#endif /* PX4_ERR */
close();
return -errno_bkp;
}
#if defined(__linux__) || defined(__PX4_LINUX)
uart_config.c_iflag &= ~(IGNBRK | BRKINT | ICRNL | INLCR | PARMRK | INPCK | ISTRIP | IXON);
uart_config.c_oflag &= ~(OCRNL | ONLCR | ONLRET | ONOCR | OFILL | OPOST);
uart_config.c_lflag &= ~(ECHO | ECHOE | ECHOK | ECHOCTL | ECHOKE | ECHONL | ICANON | IEXTEN | ISIG);
// never send SIGTTOU
uart_config.c_lflag &= ~(TOSTOP);
// ignore modem control lines
uart_config.c_cflag |= CLOCAL;
// 8 bits
uart_config.c_cflag |= CS8;
#else /* __linux__ */
// Clear ONLCR flag (which appends a CR for every LF)
uart_config.c_oflag &= ~ONLCR;
#endif
// Flow control
if (_hw_flow_control) {
// HW flow control
uart_config.c_cflag |= CRTSCTS;
uart_config.c_iflag &= ~(IXON | IXOFF | IXANY);
} else if (_sw_flow_control) {
// SW flow control
uart_config.c_cflag &= ~CRTSCTS;
uart_config.c_lflag |= (IXON | IXOFF | IXANY);
} else {
uart_config.c_cflag &= ~CRTSCTS;
uart_config.c_iflag &= ~(IXON | IXOFF | IXANY);
}
// Set baud rate
speed_t speed;
if (!baudrate_to_speed(_baudrate, &speed)) {
#ifndef PX4_ERR
printf("\033[0;31m[ micrortps_transport ]\tUART transport: ERR SET BAUD %s: Unsupported _baudrate: %d\n\tsupported examples:\n\t9600, 19200, 38400, 57600, 115200, 230400, 460800, 500000, 921600, 1000000\033[0m\n",
_uart_name, _baudrate);
#else
PX4_ERR("UART transport: ERR SET BAUD %s: Unsupported baudrate: %" PRIu32 "\n\tsupported examples:\n\t9600, 19200, 38400, 57600, 115200, 230400, 460800, 500000, 921600, 1000000\n",
_uart_name, _baudrate);
#endif /* PX4_ERR */
close();
return -EINVAL;
}
if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
int errno_bkp = errno;
#ifndef PX4_ERR
printf("\033[0;31m[ micrortps_transport ]\tUART transport: ERR SET BAUD %s: %d (%d)\033[0m\n", _uart_name, termios_state,
errno);
#else
PX4_ERR("ERR SET BAUD %s: %d (%d)", _uart_name, termios_state, errno);
#endif /* PX4_ERR */
close();
return -errno_bkp;
}
if ((termios_state = tcsetattr(_uart_fd, TCSANOW, &uart_config)) < 0) {
int errno_bkp = errno;
#ifndef PX4_ERR
printf("\033[0;31m[ micrortps_transport ]\tUART transport: ERR SET CONF %s (%d)\033[0m\n", _uart_name, errno);
#else
PX4_ERR("UART transport: ERR SET CONF %s (%d)", _uart_name, errno);
#endif /* PX4_ERR */
close();
return -errno_bkp;
}
#if defined(__linux__) || defined(__PX4_LINUX)
// For Linux, set high speed polling at the chip level. Since this routine relies on a USB latency
// change at the chip level it may fail on certain chip sets if their driver does not support this
// configuration request
{
struct serial_struct serial_ctl;
if (ioctl(_uart_fd, TIOCGSERIAL, &serial_ctl) < 0) {
printf("\033[0;31m[ micrortps_transport ]\tError while trying to read serial port configuration: %d\033[0m\n", errno);
if (ioctl(_uart_fd, TCFLSH, TCIOFLUSH) == -1) {
int errno_bkp = errno;
printf("\033[0;31m[ protocol__splitter ]\tCould not flush terminal\033[0m\n");
close();
return -errno_bkp;
}
}
serial_ctl.flags |= ASYNC_LOW_LATENCY;
if (ioctl(_uart_fd, TIOCSSERIAL, &serial_ctl) < 0) {
int errno_bkp = errno;
printf("\033[0;31m[ micrortps_transport ]\tError while trying to write serial port latency: %d\033[0m\n", errno);
close();
return -errno_bkp;
}
}
#endif /* __linux__ */
char aux[64];
bool flush = false;
while (0 < ::read(_uart_fd, (void *)&aux, 64)) {
flush = true;
#ifndef px4_usleep
usleep(1000);
#else
/* With PX4 px4_usleep() should be used. */
px4_usleep(1000);
#endif /* px4_usleep */
}
if (flush) {
#ifndef PX4_DEBUG
if (_debug) { printf("[ micrortps_transport ]\tUART transport: Flush\n"); }
#else
if (_debug) { PX4_DEBUG("UART transport: Flush"); }
#endif /* PX4_DEBUG */
} else {
#ifndef PX4_DEBUG
if (_debug) { printf("[ micrortps_transport ]\tUART transport: No flush\n"); }
#else
if (_debug) { PX4_DEBUG("UART transport: No flush"); }
#endif /* PX4_INFO */
}
_poll_fd[0].fd = _uart_fd;
_poll_fd[0].events = POLLIN;
return _uart_fd;
}
bool UART_node::fds_OK()
{
return (-1 != _uart_fd);
}
uint8_t UART_node::close()
{
if (-1 != _uart_fd) {
#ifndef PX4_WARN
printf("\033[1;33m[ micrortps_transport ]\tClosed UART.\n\033[0m");
#else
PX4_WARN("Closed UART.");
#endif /* PX4_WARN */
::close(_uart_fd);
_uart_fd = -1;
memset(&_poll_fd, 0, sizeof(_poll_fd));
}
return 0;
}
ssize_t UART_node::node_read(void *buffer, size_t len)
{
if (nullptr == buffer || !fds_OK()) {
return -1;
}
ssize_t ret = 0;
int r = poll(_poll_fd, 1, _poll_ms);
if (r == 1 && (_poll_fd[0].revents & POLLIN)) {
ret = ::read(_uart_fd, buffer, len);
}
return ret;
}
ssize_t UART_node::node_write(void *buffer, size_t len)
{
if (nullptr == buffer || !fds_OK()) {
return -1;
}
return ::write(_uart_fd, buffer, len);
}
bool UART_node::baudrate_to_speed(uint32_t bauds, speed_t *speed)
{
#ifndef B460800
#define B460800 460800
#endif
#ifndef B500000
#define B500000 500000
#endif
#ifndef B921600
#define B921600 921600
#endif
#ifndef B1000000
#define B1000000 1000000
#endif
#ifndef B1500000
#define B1500000 1500000
#endif
#ifndef B2000000
#define B2000000 2000000
#endif
switch (bauds) {
case 0: *speed = B0; break;
case 50: *speed = B50; break;
case 75: *speed = B75; break;
case 110: *speed = B110; break;
case 134: *speed = B134; break;
case 150: *speed = B150; break;
case 200: *speed = B200; break;
case 300: *speed = B300; break;
case 600: *speed = B600; break;
case 1200: *speed = B1200; break;
case 1800: *speed = B1800; break;
case 2400: *speed = B2400; break;
case 4800: *speed = B4800; break;
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;
case 460800: *speed = B460800; break;
case 500000: *speed = B500000; break;
case 921600: *speed = B921600; break;
case 1000000: *speed = B1000000; break;
case 1500000: *speed = B1500000; break;
case 2000000: *speed = B2000000; break;
#ifdef B3000000
case 3000000: *speed = B3000000; break;
#endif
#ifdef B3500000
case 3500000: *speed = B3500000; break;
#endif
#ifdef B4000000
case 4000000: *speed = B4000000; break;
#endif
default:
return false;
}
return true;
}
UDP_node::UDP_node(const char *udp_ip, uint16_t udp_port_recv,
uint16_t udp_port_send, const uint8_t sys_id, const bool debug):
Transport_node(sys_id, debug),
_sender_fd(-1),
_receiver_fd(-1),
_udp_port_recv(udp_port_recv),
_udp_port_send(udp_port_send)
{
if (nullptr != udp_ip) {
strcpy(_udp_ip, udp_ip);
}
}
UDP_node::~UDP_node()
{
close();
}
int UDP_node::init()
{
if (0 > init_receiver(_udp_port_recv) || 0 > init_sender(_udp_port_send)) {
return -1;
}
return 0;
}
bool UDP_node::fds_OK()
{
return (-1 != _sender_fd && -1 != _receiver_fd);
}
int UDP_node::init_receiver(uint16_t udp_port)
{
#if !defined (__PX4_NUTTX) || (defined (CONFIG_NET) && defined (__PX4_NUTTX))
// udp socket data
memset((char *)&_receiver_inaddr, 0, sizeof(_receiver_inaddr));
_receiver_inaddr.sin_family = AF_INET;
_receiver_inaddr.sin_port = htons(udp_port);
_receiver_inaddr.sin_addr.s_addr = htonl(INADDR_ANY);
if ((_receiver_fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
#ifndef PX4_ERR
printf("\033[0;31m[ micrortps_transport ]\tUDP transport: Create socket failed\033[0m\n");
#else
PX4_ERR("UDP transport: Create socket failed");
#endif /* PX4_ERR */
return -1;
}
#ifndef PX4_INFO
printf("[ micrortps_transport ]\tUDP transport: Trying to connect...\n");
#else
PX4_INFO("UDP transport: Trying to connect...");
#endif /* PX4_INFO */
if (bind(_receiver_fd, (struct sockaddr *)&_receiver_inaddr, sizeof(_receiver_inaddr)) < 0) {
#ifndef PX4_ERR
printf("\033[0;31m[ micrortps_transport ]\tUDP transport: Bind failed\033[0m\n");
#else
PX4_ERR("UDP transport: Bind failed");
#endif /* PX4_ERR */
return -1;
}
#ifndef PX4_INFO
printf("[ micrortps_transport ]\tUDP transport: Connected to server!\n\n");
#else
PX4_INFO("UDP transport: Connected to server!");
#endif /* PX4_INFO */
#endif /* __PX4_NUTTX */
return 0;
}
int UDP_node::init_sender(uint16_t udp_port)
{
#if !defined (__PX4_NUTTX) || (defined (CONFIG_NET) && defined (__PX4_NUTTX))
if ((_sender_fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
#ifndef PX4_ERR
printf("\033[0;31m[ micrortps_transport ]\tUDP transport: Create socket failed\033[0m\n");
#else
PX4_ERR("UDP transport: Create socket failed");
#endif /* PX4_ERR */
return -1;
}
memset((char *) &_sender_outaddr, 0, sizeof(_sender_outaddr));
_sender_outaddr.sin_family = AF_INET;
_sender_outaddr.sin_port = htons(udp_port);
if (inet_aton(_udp_ip, &_sender_outaddr.sin_addr) == 0) {
#ifndef PX4_ERR
printf("\033[0;31m[ micrortps_transport ]\tUDP transport: inet_aton() failed\033[0m\n");
#else
PX4_ERR("UDP transport: inet_aton() failed");
#endif /* PX4_ERR */
return -1;
}
#endif /* __PX4_NUTTX */
return 0;
}
uint8_t UDP_node::close()
{
#if !defined (__PX4_NUTTX) || (defined (CONFIG_NET) && defined (__PX4_NUTTX))
if (_sender_fd != -1) {
#ifndef PX4_WARN
printf("\033[1;33m[ micrortps_transport ]\tUDP transport: Closed sender socket!\033[0m\n");
#else
PX4_WARN("UDP transport: Closed sender socket!");
#endif /* PX4_WARN */
shutdown(_sender_fd, SHUT_RDWR);
::close(_sender_fd);
_sender_fd = -1;
}
if (_receiver_fd != -1) {
#ifndef PX4_WARN
printf("\033[1;33m[ micrortps_transport ]\tUDP transport: Closed receiver socket!\033[0m\n");
#else
PX4_WARN("UDP transport: Closed receiver socket!");
#endif /* PX4_WARN */
shutdown(_receiver_fd, SHUT_RDWR);
::close(_receiver_fd);
_receiver_fd = -1;
}
#endif /* __PX4_NUTTX */
return 0;
}
ssize_t UDP_node::node_read(void *buffer, size_t len)
{
if (nullptr == buffer || !fds_OK()) {
return -1;
}
ssize_t ret = 0;
#if !defined (__PX4_NUTTX) || (defined (CONFIG_NET) && defined (__PX4_NUTTX))
// Blocking call
static socklen_t addrlen = sizeof(_receiver_outaddr);
ret = recvfrom(_receiver_fd, buffer, len, 0, (struct sockaddr *)&_receiver_outaddr, &addrlen);
#endif /* !defined (__PX4_NUTTX) || (defined (CONFIG_NET) && defined (__PX4_NUTTX)) */
return ret;
}
ssize_t UDP_node::node_write(void *buffer, size_t len)
{
if (nullptr == buffer || !fds_OK()) {
return -1;
}
ssize_t ret = 0;
#if !defined (__PX4_NUTTX) || (defined (CONFIG_NET) && defined (__PX4_NUTTX))
ret = sendto(_sender_fd, buffer, len, 0, (struct sockaddr *)&_sender_outaddr, sizeof(_sender_outaddr));
#endif /* !defined (__PX4_NUTTX) || (defined (CONFIG_NET) && defined (__PX4_NUTTX)) */
return ret;
}
msg/templates/urtps/microRTPS_transport.h
-158
View File
@@ -1,158 +0,0 @@
/****************************************************************************
*
* Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima).
* Copyright (c) 2018-2021 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 of the copyright holder 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 HOLDER 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 <cstring>
#include <arpa/inet.h>
#include <poll.h>
#include <termios.h>
#define BUFFER_SIZE 1024
#define DEFAULT_UART "/dev/ttyACM0"
namespace MicroRtps {
enum class System {
FMU,
MISSION_COMPUTER
};
}
class Transport_node
{
public:
Transport_node(const uint8_t sys_id, const bool debug);
virtual ~Transport_node();
virtual int init() {return 0;}
virtual uint8_t close() {return 0;}
ssize_t read(uint8_t *topic_id, char out_buffer[], size_t buffer_len);
/**
* write a buffer
* @param topic_id
* @param buffer buffer to write: it must leave get_header_length() bytes free at the beginning. This will be
* filled with the header. length does not include get_header_length(). So buffer looks like this:
* -------------------------------------------------
* | header (leave free) | payload data |
* | get_header_length() bytes | length bytes |
* -------------------------------------------------
* @param length buffer length excluding header length
* @return length on success, <0 on error
*/
ssize_t write(const uint8_t topic_id, char buffer[], size_t length);
/** Get the Length of struct Header to make headroom for the size of struct Header along with payload */
size_t get_header_length();
private:
struct __attribute__((packed)) Header {
char marker[3];
uint8_t topic_id;
uint8_t sys_id;
uint8_t seq;
uint8_t payload_len_h;
uint8_t payload_len_l;
uint8_t crc_h;
uint8_t crc_l;
};
protected:
virtual ssize_t node_read(void *buffer, size_t len) = 0;
virtual ssize_t node_write(void *buffer, size_t len) = 0;
virtual bool fds_OK() = 0;
uint16_t crc16_byte(uint16_t crc, const uint8_t data);
uint16_t crc16(uint8_t const *buffer, size_t len);
uint32_t _rx_buff_pos;
char _rx_buffer[BUFFER_SIZE]{};
bool _debug;
uint8_t _sys_id;
uint8_t _seq_number{0};
};
class UART_node: public Transport_node
{
public:
UART_node(const char *uart_name, const uint32_t baudrate,
const uint32_t poll_ms, const bool hw_flow_control,
const bool sw_flow_control, const uint8_t sys_id,
const bool debug);
virtual ~UART_node();
int init();
uint8_t close();
protected:
ssize_t node_read(void *buffer, size_t len);
ssize_t node_write(void *buffer, size_t len);
bool fds_OK();
bool baudrate_to_speed(uint32_t bauds, speed_t *speed);
int _uart_fd;
char _uart_name[64]{};
uint32_t _baudrate;
uint32_t _poll_ms;
bool _hw_flow_control{false};
bool _sw_flow_control{false};
struct pollfd _poll_fd[1]{};
};
class UDP_node: public Transport_node
{
public:
UDP_node(const char *udp_ip, uint16_t udp_port_recv, uint16_t udp_port_send,
const uint8_t sys_id, const bool debug);
virtual ~UDP_node();
int init();
uint8_t close();
protected:
int init_receiver(uint16_t udp_port);
int init_sender(uint16_t udp_port);
ssize_t node_read(void *buffer, size_t len);
ssize_t node_write(void *buffer, size_t len);
bool fds_OK();
int _sender_fd;
int _receiver_fd;
char _udp_ip[16]{};
uint16_t _udp_port_recv;
uint16_t _udp_port_send;
struct sockaddr_in _sender_outaddr;
struct sockaddr_in _receiver_inaddr;
struct sockaddr_in _receiver_outaddr;
};
msg/templates/urtps/msg.idl.em
-154
View File
@@ -1,154 +0,0 @@
@###############################################
@#
@# ROS message to IDL converter
@#
@# EmPy template for generating <msg>.idl files
@#
@###############################################
@# Start of Template
@#
@# Context:
@# - spec (msggen.MsgSpec) Parsed specification of the .msg file
@################################################################################
@#
@# Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima).
@# Copyright (c) 2018-2021 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 of the copyright holder 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 HOLDER 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.
@#
@################################################################################
@{
import genmsg.msgs
from packaging import version
from px_generate_uorb_topic_helper import * # this is in Tools/
builtin_types = set()
array_types = set()
topic = alias if alias else spec.short_name
}@
@#################################################
@# Searching for serialize function per each field
@#################################################
@{
def get_include_directives(spec):
include_directives = set()
for field in (spec.parsed_fields()):
if genmsg.msgs.is_valid_constant_type(genmsg.msgs.bare_msg_type(field.type)):
continue
builtin_type = str(field.base_type).replace('px4/', '')
if version.parse(fastrtps_version) <= version.parse('1.7.2'):
include_directive = '#include "%s_.idl"' % builtin_type
else:
include_directive = '#include "%s.idl"' % builtin_type
builtin_types.add(builtin_type)
include_directives.add(include_directive)
return sorted(include_directives)
def get_idl_type_name(field_type):
if field_type in type_idl_map:
return type_idl_map[field_type]
else:
(package, name) = genmsg.names.package_resource_name(field_type)
return name
def add_msg_field(field):
if (not field.is_header):
if field.is_array:
if version.parse(fastrtps_version) <= version.parse('1.7.2'):
print(' {0}__{1}_array_{2} {3}_;'.format(topic, str(get_idl_type_name(field.base_type)).replace(" ", "_"), str(field.array_len), field.name))
else:
print(' {0}__{1}_array_{2} {3};'.format(topic, str(get_idl_type_name(field.base_type)).replace(" ", "_"), str(field.array_len), field.name))
else:
if version.parse(fastrtps_version) <= version.parse('1.7.2'):
base_type = get_idl_type_name(field.base_type) + "_" if get_idl_type_name(field.base_type) in builtin_types else get_idl_type_name(field.base_type)
else:
base_type = get_idl_type_name(field.base_type) if get_idl_type_name(field.base_type) in builtin_types else get_idl_type_name(field.base_type)
print(' {0} {1}_;'.format(base_type, field.name))
def add_msg_fields():
for field in spec.parsed_fields():
add_msg_field(field)
def add_array_typedefs():
for field in spec.parsed_fields():
if not field.is_header and field.is_array:
if version.parse(fastrtps_version) <= version.parse('1.7.2'):
base_type = get_idl_type_name(field.base_type) + "_" if get_idl_type_name(field.base_type) in builtin_types else get_idl_type_name(field.base_type)
else:
base_type = get_idl_type_name(field.base_type) if get_idl_type_name(field.base_type) in builtin_types else get_idl_type_name(field.base_type)
array_type = 'typedef {0} {1}__{2}_array_{3}[{4}];'.format(base_type, topic, get_idl_type_name(field.base_type).replace(" ", "_"), field.array_len, field.array_len)
if array_type not in array_types:
array_types.add(array_type)
for atype in array_types:
print(atype)
def add_msg_constants():
sorted_constants = sorted(spec.constants,
key=sizeof_field_type, reverse=True)
for constant in sorted_constants:
print('const {0} {1}__{2} = {3};'.format(get_idl_type_name(constant.type), topic, constant.name, constant.val))
}
#ifndef __@(topic)__idl__
#define __@(topic)__idl__
@#############################
@# Include dependency messages
@#############################
@[for line in get_include_directives(spec)]@
@(line)@
@[end for]@
@# Constants
@add_msg_constants()
@# Array types
@add_array_typedefs()
@[if version.parse(fastrtps_version) <= version.parse('1.7.2')]@
struct @(topic)_
@[else]@
struct @(topic)
@[end if]@
{
@add_msg_fields()
@[if version.parse(fastrtps_version) <= version.parse('1.7.2')]@
}; // struct @(topic)_
#pragma keylist @(topic)_
@[else]@
}; // struct @(topic)
#pragma keylist @(topic)
@[end if]@
#endif // __@(topic)__idl__