yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 01:12:11 +00:00
Code Issues Packages Projects Releases Wiki Activity

microRTPS: client: match the code style from the PX4 Firmware

Browse Source
This commit is contained in:
TSC21
2021-05-01 12:14:12 +02:00
committed by Nuno Marques
parent 63571b3e3f
commit 320b414511
3 changed files with 130 additions and 128 deletions
Download Patch File Download Diff File Expand all files Collapse all files

254
msg/templates/uorb_microcdr/microRTPS_client.cpp.em
View File

@@ -27,7 +27,7 @@ receive_base_types = [s.short_name for idx, s in enumerate(spec) if scope[idx] =
/****************************************************************************
*
* Copyright (c) 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima).
* Copyright (c) 2018-2019 PX4 Development Team. All rights reserved.
* 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:
@@ -76,184 +76,186 @@ receive_base_types = [s.short_name for idx, s in enumerate(spec) if scope[idx] =
#include <uORB_microcdr/topics/@(topic).h>
@[end for]@
void* send(void *data);
void *send(void *data);
uint8_t last_remote_msg_seq = 0;
uint8_t last_msg_seq = 0;
@[if recv_topics]@
// Publishers for received messages
struct RcvTopicsPubs
{
struct RcvTopicsPubs {
@[ for idx, topic in enumerate(recv_topics)]@
uORB::Publication<@(receive_base_types[idx])_s> @(topic)_pub{ORB_ID(@(topic))};
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
{
struct SendTopicsSubs {
@[ for idx, topic in enumerate(send_topics)]@
uORB::Subscription @(topic)_sub{ORB_ID(@(topic))};
uORB::Subscription @(topic)_sub{ORB_ID(@(topic))};
@[ end for]@
};
@[end if]@
@[if send_topics]@
void* send(void* /*unused*/)
void *send(void * /*unused*/)
{
char data_buffer[BUFFER_SIZE] = {};
uint64_t sent = 0, total_sent = 0;
int loop = 0, read = 0;
uint32_t length = 0;
size_t header_length = 0;
SendTopicsSubs *subs = new SendTopicsSubs();
char data_buffer[BUFFER_SIZE] = {};
uint64_t sent = 0, total_sent = 0;
int loop = 0, read = 0;
uint32_t length = 0;
size_t header_length = 0;
SendTopicsSubs *subs = new SendTopicsSubs();
// 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);
// 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);
struct timespec begin;
px4_clock_gettime(CLOCK_REALTIME, &begin);
struct timespec begin;
px4_clock_gettime(CLOCK_REALTIME, &begin);
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.sys_id == 0 && @(topic)_data.seq != last_remote_msg_seq && @(topic)_data.tc1 == 0) {
last_remote_msg_seq = @(topic)_data.seq;
while (!_should_exit_task) {
@[ for idx, topic in enumerate(send_topics)]@
@(send_base_types[idx])_s @(topic)_data;
@(topic)_data.timestamp = hrt_absolute_time();
@(topic)_data.sys_id = 1;
@(topic)_data.seq = last_msg_seq;
@(topic)_data.tc1 = hrt_absolute_time() * 1000ULL;
@(topic)_data.ts1 = @(topic)_data.ts1;
if (subs->@(topic)_sub.update(&@(topic)_data))
{
@[ if topic == 'Timesync' or topic == 'timesync']@
if (@(topic)_data.sys_id == 0 && @(topic)_data.seq != last_remote_msg_seq && @(topic)_data.tc1 == 0) {
last_remote_msg_seq = @(topic)_data.seq;
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>(@(rtps_message_id(ids, topic))), data_buffer, length)))
{
total_sent += read;
++sent;
}
@[if topic == 'Timesync' or topic == 'timesync']@
}
@[end if]@
}
}
@[end for]@
px4_usleep(_options.sleep_ms * 1000);
++loop;
}
@(topic)_data.timestamp = hrt_absolute_time();
@(topic)_data.sys_id = 1;
@(topic)_data.seq = last_msg_seq;
@(topic)_data.tc1 = hrt_absolute_time() * 1000ULL;
@(topic)_data.ts1 = @(topic)_data.ts1;
struct timespec end;
px4_clock_gettime(CLOCK_REALTIME, &end);
double elapsed_secs = end.tv_sec - begin.tv_sec + (end.tv_nsec - begin.tv_nsec) / 1e9;
PX4_INFO("SENT: %" PRIu64 " messages in %d LOOPS, %" PRIu64 " bytes in %.03f seconds - %.02fKB/s",
sent, loop, total_sent, elapsed_secs, total_sent / (1e3 * elapsed_secs));
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);
delete subs;
if (0 < (read = transport_node->write(static_cast<char>(@(rtps_message_id(ids, topic))), data_buffer, length))) {
total_sent += read;
++sent;
}
return nullptr;
@[ if topic == 'Timesync' or topic == 'timesync']@
}
@[ end if]@
}
@[ end for]@
px4_usleep(_options.sleep_ms * 1000);
++loop;
}
struct timespec end;
px4_clock_gettime(CLOCK_REALTIME, &end);
double elapsed_secs = end.tv_sec - begin.tv_sec + (end.tv_nsec - begin.tv_nsec) / 1e9;
PX4_INFO("SENT: %" PRIu64 " messages in %d LOOPS, %" PRIu64 " bytes in %.03f seconds - %.02fKB/s",
sent, loop, total_sent, elapsed_secs, total_sent / (1e3 * elapsed_secs));
delete subs;
return nullptr;
}
static int launch_send_thread(pthread_t &sender_thread)
{
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);
pthread_create(&sender_thread, &sender_thread_attr, send, nullptr);
if (pthread_setname_np(sender_thread, "micrortps_client_send"))
{
PX4_ERR("Could not set pthread name (%d)", errno);
}
pthread_attr_destroy(&sender_thread_attr);
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);
pthread_create(&sender_thread, &sender_thread_attr, send, nullptr);
return 0;
if (pthread_setname_np(sender_thread, "micrortps_client_send")) {
PX4_ERR("Could not set pthread name (%d)", errno);
}
pthread_attr_destroy(&sender_thread_attr);
return 0;
}
@[end if]@
void micrortps_start_topics(struct timespec &begin, uint64_t &total_read, uint64_t &received, int &loop)
{
@[if recv_topics]@
char data_buffer[BUFFER_SIZE] = {};
int read = 0;
uint8_t topic_ID = 255;
RcvTopicsPubs *pubs = new RcvTopicsPubs();
char data_buffer[BUFFER_SIZE] = {};
int read = 0;
uint8_t topic_ID = 255;
RcvTopicsPubs *pubs = new RcvTopicsPubs();
// Set the main task name to 'micrortps_client_rcv' in case there is
// data to receive
px4_prctl(PR_SET_NAME, "micrortps_client_rcv", px4_getpid());
// Set the main task name to 'micrortps_client_rcv' in case there is
// data to receive
px4_prctl(PR_SET_NAME, "micrortps_client_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);
// ucdrBuffer to deserialize using the user defined buffer
ucdrBuffer reader;
ucdr_init_buffer(&reader, reinterpret_cast<uint8_t *>(data_buffer), BUFFER_SIZE);
@[end if]@
px4_clock_gettime(CLOCK_REALTIME, &begin);
_should_exit_task = false;
px4_clock_gettime(CLOCK_REALTIME, &begin);
_should_exit_task = false;
@[if send_topics]@
// create a thread for sending data to the simulator
pthread_t sender_thread;
launch_send_thread(sender_thread);
// create a thread for sending data to the simulator
pthread_t sender_thread;
launch_send_thread(sender_thread);
@[end if]@
while (!_should_exit_task)
{
while (!_should_exit_task) {
@[if recv_topics]@
while (0 < (read = transport_node->read(&topic_ID, data_buffer, BUFFER_SIZE)))
{
total_read += read;
uint64_t read_time = hrt_absolute_time();
switch (topic_ID)
{
@[for idx, topic in enumerate(recv_topics)]@
case @(rtps_message_id(ids, topic)):
{
@(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;
}
}
while (0 < (read = transport_node->read(&topic_ID, data_buffer, BUFFER_SIZE))) {
total_read += read;
uint64_t read_time = hrt_absolute_time();
switch (topic_ID) {
@[ for idx, topic in enumerate(recv_topics)]@
case @(rtps_message_id(ids, topic)): {
@(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]@
// loop forever if informed loop number is negative
if (_options.loops >= 0 && loop >= _options.loops) break;
// loop forever if informed loop number is negative
if (_options.loops >= 0 && loop >= _options.loops) { break; }
px4_usleep(_options.sleep_ms * 1000);
++loop;
}
px4_usleep(_options.sleep_ms * 1000);
++loop;
}
@[if recv_topics]@
delete pubs;
delete pubs;
@[end if]@
@[if send_topics]@
_should_exit_task = true;
pthread_join(sender_thread, nullptr);
_should_exit_task = true;
pthread_join(sender_thread, nullptr);
@[end if]@
}

2
msg/templates/uorb_microcdr/uORBTopics.cpp.em
View File

@@ -13,7 +13,7 @@
@###############################################
/****************************************************************************
*
* Copyright (C) 2013-2015 PX4 Development Team. All rights reserved.
* 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

2
msg/templates/uorb_microcdr/uORBTopics.hpp.em
View File

@@ -13,7 +13,7 @@
@###############################################
/****************************************************************************
*
* Copyright (C) 2020 PX4 Development Team. All rights reserved.
* 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