fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-28 05:23:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

RTPS and micro-CDR build system cleanup (#8084)

Browse Source
This commit is contained in:
Daniel Agar
2017-10-11 13:05:44 -04:00
committed by GitHub
parent bb9f8c3b44
commit 651df03f76
57 changed files with 405 additions and 188 deletions
Download Patch File Download Diff File Expand all files Collapse all files
msg/templates/uorb/microRTPS_client.cpp.template
-220
View File
@@ -1,220 +0,0 @@
@###############################################
@#
@# EmPy template for generating microRTPS_client.cpp file
@#
@###############################################
@# Start of Template
@#
@# Context:
@# - msgs (List) list of all msg files
@# - multi_topics (List) list of all multi-topic names
@###############################################
@{
import genmsg.msgs
import gencpp
from px_generate_uorb_topic_helper import * # this is in Tools/
from px_generate_uorb_topic_files import MsgScope # this is in Tools/
from uorb_rtps_message_ids import * # this is in Tools/
topic_names = [single_spec.short_name for single_spec in spec]
send_topics = [s.short_name for idx, s in enumerate(spec) if scope[idx] == MsgScope.SEND]
recv_topics = [s.short_name for idx, s in enumerate(spec) if scope[idx] == MsgScope.RECEIVE]
}@
/****************************************************************************
*
* Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima).
*
* 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 <cinttypes>
#include <cstdio>
#include <ctime>
#include <pthread.h>
#include <microcdr/microCdr.h>
#include <px4_time.h>
#include <uORB/uORB.h>
@[for topic in list(set(topic_names))]@
#include <uORB/topics/@(topic).h>
@[end for]@
void* send(void *data);
@[if send_topics]@
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;
/* subscribe to topics */
int fds[@(len(send_topics))] = {};
// orb_set_interval statblish an update interval period in milliseconds.
@[for idx, topic in enumerate(send_topics)]@
fds[@(idx)] = orb_subscribe(ORB_ID(@(topic)));
orb_set_interval(fds[@(idx)], _options.update_time_ms);
@[end for]@
// microBuffer to serialized using the user defined buffer
struct microBuffer microBufferWriter;
initStaticAlignedBuffer(data_buffer, BUFFER_SIZE, &microBufferWriter);
// microCDR structs for managing the microBuffer
struct microCDR microCDRWriter;
initMicroCDR(&microCDRWriter, &microBufferWriter);
struct timespec begin;
px4_clock_gettime(CLOCK_REALTIME, &begin);
while (!_should_exit_task)
{
bool updated;
@[for idx, topic in enumerate(send_topics)]@
orb_check(fds[@(idx)], &updated);
if (updated)
{
// obtained data for the file descriptor
struct @(topic)_s data;
// copy raw data into local buffer
if (orb_copy(ORB_ID(@(topic)), fds[@(idx)], &data) == 0) {
serialize_@(topic)(&data, data_buffer, &length, &microCDRWriter);
if (0 < (read = transport_node->write((char)@(message_id(topic)), data_buffer, length)))
{
total_sent += read;
++sent;
}
}
}
@[end for]@
usleep(_options.sleep_ms*1000);
++loop;
}
struct timespec end;
px4_clock_gettime(CLOCK_REALTIME, &end);
double elapsed_secs = double(end.tv_sec - begin.tv_sec) + double(end.tv_nsec - begin.tv_nsec)/double(1000000000);
printf("\nSENT: %" PRIu64 " messages in %d LOOPS, %" PRIu64 " bytes in %.03f seconds - %.02fKB/s\n",
sent, loop, total_sent, elapsed_secs, (double)total_sent/(1000*elapsed_secs));
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(4000));
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);
pthread_attr_destroy(&sender_thread_attr);
return 0;
}
@[end if]@
void micrortps_start_topics(struct timespec &begin, int &total_read, uint32_t &received, int &loop)
{
@[if recv_topics]@
char data_buffer[BUFFER_SIZE] = {};
int read = 0;
uint8_t topic_ID = 255;
// Declare received topics
@[for topic in recv_topics]@
struct @(topic)_s @(topic)_data;
orb_advert_t @(topic)_pub = nullptr;
@[end for]@
// microBuffer to deserialized using the user defined buffer
struct microBuffer microBufferReader;
initDeserializedAlignedBuffer(data_buffer, BUFFER_SIZE, &microBufferReader);
// microCDR structs for managing the microBuffer
struct microCDR microCDRReader;
initMicroCDR(&microCDRReader, &microBufferReader);
@[end if]@
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);
@[end if]@
while (!_should_exit_task)
{
@[if recv_topics]@
while (0 < (read = transport_node->read(&topic_ID, data_buffer, BUFFER_SIZE)))
{
total_read += read;
switch (topic_ID)
{
@[for topic in recv_topics]@
case @(message_id(topic)):
{
deserialize_@(topic)(&@(topic)_data, data_buffer, &microCDRReader);
if (!@(topic)_pub) {
@(topic)_pub = orb_advertise(ORB_ID(@(topic)), &@(topic)_data);
} else {
orb_publish(ORB_ID(@(topic)), @(topic)_pub, &@(topic)_data);
}
++received;
}
break;
@[end for]@
default:
printf("Unexpected topic ID\n");
break;
}
}
@[end if]@
// loop forever if informed loop number is negative
if (_options.loops >= 0 && loop >= _options.loops) break;
usleep(_options.sleep_ms*1000);
++loop;
}
@[if send_topics]@
_should_exit_task = true;
pthread_join(sender_thread, nullptr);
@[end if]@
}
msg/templates/uorb/msg.cpp.template
-99
View File
@@ -64,9 +64,6 @@ struct_size, padding_end_size = add_padding_bytes(sorted_fields, search_path)
topic_fields = ["uint64_t timestamp"]+["%s %s" % (convert_type(field.type), field.name) for field in sorted_fields]
}@
#include <px4_config.h>
#include <drivers/drv_orb_dev.h>
#include <microcdr/microCdr.h>
#include <uORB/topics/@(topic_name).h>
@# join all msg files in one line e.g: "float[3] position;float[3] velocity;bool armed"
@@ -76,99 +73,3 @@ 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);
@[end for]
@#################################################
@# 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("\tserialize"+str(get_serialization_type_name(field.type))+"(input->"+scope_name+str(field.name)+", microCDRWriter);")
else:
print("\tserialize"+str(get_serialization_type_name(field.base_type))+"Array(input->"+scope_name+str(field.name)+", "+str(field.array_len)+", microCDRWriter);")
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("\tdeserialize"+str(get_serialization_type_name(field.type))+"(&output->"+scope_name+str(field.name)+", microCDRReader);")
else:
for i in range(field.array_len):
print("\tdeserialize"+str(get_serialization_type_name(field.base_type))+"(&output->"+scope_name+str(field.name)+ str('[%d]' %i) +", microCDRReader);")
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():
# sort fields (using a stable sort) as in the declaration of the type
sorted_fields = sorted(spec.parsed_fields(), key=sizeof_field_type, reverse=True)
add_serialize_functions(sorted_fields, "")
def add_code_to_deserialize():
# sort fields (using a stable sort) as in the declaration of the type
sorted_fields = sorted(spec.parsed_fields(), key=sizeof_field_type, reverse=True)
add_deserialize_functions(sorted_fields, "")
}@
void serialize_@(topic_name)(const struct @(uorb_struct) *input, char *output, uint32_t *length, struct microCDR *microCDRWriter)
{
if (nullptr == input || nullptr == output || nullptr == length || nullptr == microCDRWriter) return;
resetStaticMicroCDRForSerialize(microCDRWriter);
@add_code_to_serialize()
(*length) = microCDRWriter->m_microBuffer->m_serializedBuffer;
}
void deserialize_@(topic_name)(struct @(uorb_struct) *output, char *input, struct microCDR *microCDRReader)
{
if (nullptr == output || nullptr == input || nullptr == microCDRReader) return;
resetStaticMicroCDRForDeserialize(microCDRReader);
@add_code_to_deserialize()
}
msg/templates/uorb/msg.h.template
-12
View File
@@ -65,12 +65,6 @@ topic_name = spec.short_name
@##############################
@# Generic Includes
@##############################
#include <stdint.h>
#ifdef __cplusplus
#include <cstring>
#else
#include <string.h>
#endif
#include <uORB/uORB.h>
@@ -108,8 +102,6 @@ def print_parsed_fields():
print_field_def(field)
}@
struct microCDR;
#ifdef __cplusplus
@#class @(uorb_struct) {
struct __EXPORT @(uorb_struct) {
@@ -137,10 +129,6 @@ for constant in spec.constants:
#endif
};
void serialize_@(topic_name)(const struct @(uorb_struct) *input, char *output, uint32_t *length, struct microCDR *microCDRWriter);
void deserialize_@(topic_name)(struct @(uorb_struct) *output, char *input, struct microCDR *microCDRReader);
/* register this as object request broker structure */
@[for multi_topic in topics]@
ORB_DECLARE(@multi_topic);