fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-17 20:53:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Fixing rebase conflicts

Browse Source
This commit is contained in:
Vicente Monge
2017-08-01 22:32:43 +02:00
committed by Lorenz Meier
parent d69c53827f
commit c85039e413
65 changed files with 4091 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
msg/templates/uorb/microRTPS_client.cpp.template
+406
View File
@@ -0,0 +1,406 @@
@###############################################
@#
@# 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 message_id 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 <px4_config.h>
#include <px4_getopt.h>
#include <px4_tasks.h>
#include <px4_posix.h>
#include <unistd.h>
#include <stdio.h>
#include <string.h>
#include <termios.h>
#include <ctime>
#include <pthread.h>
#include <microcdr/microCdr.h>
#include <uORB/uORB.h>
#include "microRTPS_transport.h"
@[for topic in list(set(topic_names))]@
#include <uORB/topics/@(topic).h>
@[end for]@
#define BUFFER_SIZE 1024
#define UPDATE_TIME_MS 0
#define LOOPS 10000
#define SLEEP_MS 1
#define BAUDRATE 460800
#define DEVICE "/dev/ttyACM0"
#define POLL_MS 1
#define DEFAULT_RECV_PORT 2019
#define DEFAULT_SEND_PORT 2020
extern "C" __EXPORT int micrortps_client_main(int argc, char *argv[]);
void* send(void *data);
struct options {
enum class eTransports
{
UART,
UDP
};
eTransports transport = options::eTransports::UART;
char device[64] = DEVICE;
int update_time_ms = UPDATE_TIME_MS;
int loops = LOOPS;
int sleep_ms = SLEEP_MS;
uint32_t baudrate = BAUDRATE;
int poll_ms = POLL_MS;
uint16_t recv_port = DEFAULT_RECV_PORT;
uint16_t send_port = DEFAULT_SEND_PORT;
} _options;
static int _rtps_task = -1;
static bool _should_exit_task = false;
Transport_node *transport_node = nullptr;
static void usage(const char *name)
{
PX4_INFO("usage: %s start|stop [options]\n\n"
" -t <transport> [UART|UDP] Default UART\n"
" -d <device> UART device. Default /dev/ttyACM0\n"
" -u <update_time_ms> Time in ms for uORB subscribed topics update. Default 0\n"
" -l <loops> How many iterations will this program have. -1 for infinite. Default 10000\n"
" -w <sleep_time_ms> Time in ms for which each iteration sleep. Default 1ms\n"
" -b <baudrate> UART device baudrate. Default 460800\n"
" -p <poll_ms> Time in ms to poll over UART. Default 1ms\n"
" -r <reception port> UDP port for receiving. Default 2019\n"
" -s <sending port> UDP port for sending. Default 2020\n",
name);
}
static int parse_options(int argc, char *argv[])
{
int ch;
int myoptind = 1;
const char *myoptarg = nullptr;
while ((ch = px4_getopt(argc, argv, "t:d:u:l:w:b:p:r:s:", &myoptind, &myoptarg)) != EOF)
{
switch (ch)
{
case 't': _options.transport = strcmp(myoptarg, "UDP") == 0?
options::eTransports::UDP
:options::eTransports::UART; break;
case 'd': if (nullptr != myoptarg) strcpy(_options.device, myoptarg); break;
case 'u': _options.update_time_ms = strtol(myoptarg, nullptr, 10); break;
case 'l': _options.loops = strtol(myoptarg, nullptr, 10); break;
case 'w': _options.sleep_ms = strtol(myoptarg, nullptr, 10); break;
case 'b': _options.baudrate = strtoul(optarg, nullptr, 10); break;
case 'p': _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;
default:
usage(argv[1]);
return -1;
}
}
return 0;
}
@[if send_topics]@
void* send(void*)
{
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;
clock_gettime(0, &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
orb_copy(ORB_ID(@(topic)), fds[@(idx)], &data);
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;
clock_gettime(0, &end);
double elapsed_secs = double(end.tv_sec - begin.tv_sec) + double(end.tv_nsec - begin.tv_nsec)/double(1000000000);
printf("\nSENT: %lu messages in %d LOOPS, %llu bytes in %.03f seconds - %.02fKB/s\n",
(unsigned long)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]@
static int micrortps_start(int argc, char *argv[])
{
if (0 > parse_options(argc, argv))
{
printf("EXITING...\n");
_rtps_task = -1;
return -1;
}
switch (_options.transport)
{
case options::eTransports::UART:
{
transport_node = new UART_node(_options.device, _options.baudrate, _options.poll_ms);
printf("\nUART transport: device: %s; baudrate: %d; sleep: %dms; poll: %dms\n\n",
_options.device, _options.baudrate, _options.sleep_ms, _options.poll_ms);
}
break;
case options::eTransports::UDP:
{
transport_node = new UDP_node(_options.recv_port, _options.send_port);
printf("\nUDP transport: recv port: %u; send port: %u; sleep: %dms\n\n",
_options.recv_port, _options.send_port, _options.sleep_ms);
}
break;
default:
_rtps_task = -1;
printf("EXITING...\n");
return -1;
}
if (0 > transport_node->init())
{
printf("EXITING...\n");
_rtps_task = -1;
return -1;
}
sleep(1);
@[if recv_topics]@
char data_buffer[BUFFER_SIZE] = {};
int read = 0;
char 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]@
int total_read = 0;
uint32_t received = 0;
struct timespec begin;
clock_gettime(0, &begin);
int loop = 0;
_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, 0);
@[end if]@
struct timespec end;
clock_gettime(0, &end);
double elapsed_secs = double(end.tv_sec - begin.tv_sec) + double(end.tv_nsec - begin.tv_nsec)/double(1000000000);
printf("RECEIVED: %lu messages in %d LOOPS, %d bytes in %.03f seconds - %.02fKB/s\n\n",
(unsigned long)received, loop, total_read, elapsed_secs, (double)total_read/(1000*elapsed_secs));
delete transport_node;
transport_node = nullptr;
PX4_INFO("exiting");
fflush(stdout);
_rtps_task = -1;
return 0;
}
int micrortps_client_main(int argc, char *argv[])
{
if (argc < 2)
{
usage(argv[0]);
return -1;
}
if (!strcmp(argv[1], "start"))
{
if (_rtps_task != -1)
{
PX4_INFO("Already running");
return -1;
}
_rtps_task = px4_task_spawn_cmd("rtps",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
4096,
(px4_main_t) micrortps_start,
(char *const *)argv);
if (_rtps_task < 0)
{
PX4_WARN("Could not start task");
_rtps_task = -1;
return -1;
}
return 0;
}
if (!strcmp(argv[1], "stop"))
{
if (_rtps_task == -1)
{
PX4_INFO("Not running");
return -1;
}
_should_exit_task = true;
if (nullptr != transport_node) transport_node->close();
return 0;
}
usage(argv[0]);
return -1;
}
msg/templates/uorb/msg.cpp.template
+99 -1
View File
@@ -64,9 +64,11 @@ 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"
@# This is used for the logger
constexpr char __orb_@(topic_name)_fields[] = "@( ";".join(topic_fields) );";
@@ -75,3 +77,99 @@ constexpr char __orb_@(topic_name)_fields[] = "@( ";".join(topic_fields) );";
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 xrange(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 xrange(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 xrange(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
+7
View File
@@ -108,6 +108,8 @@ def print_parsed_fields():
print_field_def(field)
}@
struct microCDR;
#ifdef __cplusplus
@#class @(uorb_struct) {
struct __EXPORT @(uorb_struct) {
@@ -118,6 +120,7 @@ struct @(uorb_struct) {
@# timestamp at the beginning of each topic is required for logger
uint64_t timestamp; // required for logger
@print_parsed_fields()
#ifdef __cplusplus
@# Constants again c++-ified
@{
@@ -134,6 +137,10 @@ 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);
msg/templates/urtps/Publisher.cxx.template
+154
View File
@@ -0,0 +1,154 @@
@###############################################
@#
@# EmPy template for generating <msg>_uRTPS_UART.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/
topic = spec.short_name
}@
/****************************************************************************
*
* 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.
*
****************************************************************************/
/*!
* @@file @(topic)_Publisher.cpp
* This file contains the implementation of the publisher functions.
*
* This file was generated by the tool fastcdrgen.
*/
#include <fastrtps/participant/Participant.h>
#include <fastrtps/attributes/ParticipantAttributes.h>
#include <fastrtps/publisher/Publisher.h>
#include <fastrtps/attributes/PublisherAttributes.h>
#include <fastrtps/Domain.h>
#include <fastrtps/utils/eClock.h>
#include "@(topic)_Publisher.h"
@(topic)_Publisher::@(topic)_Publisher() : mp_participant(nullptr), mp_publisher(nullptr) {}
@(topic)_Publisher::~@(topic)_Publisher() { Domain::removeParticipant(mp_participant);}
bool @(topic)_Publisher::init()
{
// Create RTPSParticipant
ParticipantAttributes PParam;
PParam.rtps.builtin.domainId = 0;
PParam.rtps.builtin.leaseDuration = c_TimeInfinite;
PParam.rtps.setName("Participant_publisher"); //You can put here the name you want
mp_participant = Domain::createParticipant(PParam);
if(mp_participant == nullptr)
return false;
//Register the type
Domain::registerType(mp_participant,(TopicDataType*) &myType);
// Create Publisher
PublisherAttributes Wparam;
Wparam.topic.topicKind = NO_KEY;
Wparam.topic.topicDataType = myType.getName(); //This type MUST be registered
Wparam.topic.topicName = "@(topic)_PubSubTopic";
mp_publisher = Domain::createPublisher(mp_participant,Wparam,(PublisherListener*)&m_listener);
if(mp_publisher == nullptr)
return false;
//std::cout << "Publisher created, waiting for Subscribers." << std::endl;
return true;
}
void @(topic)_Publisher::PubListener::onPublicationMatched(Publisher* pub,MatchingInfo& info)
{
if (info.status == MATCHED_MATCHING)
{
n_matched++;
std::cout << "Publisher matched" << std::endl;
}
else
{
n_matched--;
std::cout << "Publisher unmatched" << std::endl;
}
}
void @(topic)_Publisher::run()
{
while(m_listener.n_matched == 0)
{
eClock::my_sleep(250); // Sleep 250 ms
}
// Publication code
@(topic)_ st;
/* Initialize your structure here */
int msgsent = 0;
char ch = 'y';
do
{
if(ch == 'y')
{
mp_publisher->write(&st); ++msgsent;
std::cout << "Sending sample, count=" << msgsent << ", send another sample?(y-yes,n-stop): ";
}
else if(ch == 'n')
{
std::cout << "Stopping execution " << std::endl;
break;
}
else
{
std::cout << "Command " << ch << " not recognized, please enter \"y/n\":";
}
}while(std::cin >> ch);
}
void @(topic)_Publisher::publish(@(topic)_* st)
{
mp_publisher->write(st);
}
msg/templates/urtps/Publisher.h.template
+92
View File
@@ -0,0 +1,92 @@
@###############################################
@#
@# EmPy template for generating <msg>_uRTPS_UART.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/
topic = spec.short_name
}@
/****************************************************************************
*
* 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.
*
****************************************************************************/
/*!
* @@file @(topic)_Publisher.h
* This header file contains the declaration of the publisher functions.
*
* This file was generated by the tool fastcdrgen.
*/
#ifndef _@(topic)__PUBLISHER_H_
#define _@(topic)__PUBLISHER_H_
#include <fastrtps/fastrtps_fwd.h>
#include <fastrtps/publisher/PublisherListener.h>
#include "@(topic)_PubSubTypes.h"
using namespace eprosima::fastrtps;
class @(topic)_Publisher
{
public:
@(topic)_Publisher();
virtual ~@(topic)_Publisher();
bool init();
void run();
void publish(@(topic)_* 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)_PubSubType myType;
};
#endif // _@(topic)__PUBLISHER_H_
msg/templates/urtps/RtpsTopics.cxx.template
+159
View File
@@ -0,0 +1,159 @@
@###############################################
@#
@# EmPy template for generating RtpsTopics.cxx file
@#
@###############################################
@# Start of Template
@#
@# Context:
@# - msgs (List) list of all msg files
@###############################################
@{
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 message_id 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 "RtpsTopics.h"
bool RtpsTopics::init()
{
@[if recv_topics]@
// Initialise subscribers
@[for topic in recv_topics]@
if (_@(topic)_sub.init()) {
std::cout << "@(topic) subscriber started" << std::endl;
} else {
std::cout << "ERROR starting @(topic) subscriber" << std::endl;
return false;
}
@[end for]@
@[end if]@
@[if send_topics]@
// Initialise publishers
@[for topic in send_topics]@
if (_@(topic)_pub.init()) {
std::cout << "@(topic) publisher started" << std::endl;
} else {
std::cout << "ERROR starting @(topic) publisher" << std::endl;
return false;
}
@[end for]@
@[end if]@
return true;
}
@[if send_topics]@
void RtpsTopics::publish(char topic_ID, char data_buffer[], size_t len)
{
switch (topic_ID)
{
@[for topic in send_topics]@
case @(message_id(topic)): // @(topic)
{
@(topic)_ st;
eprosima::fastcdr::FastBuffer cdrbuffer(data_buffer, len);
eprosima::fastcdr::Cdr cdr_des(cdrbuffer);
st.deserialize(cdr_des);
_@(topic)_pub.publish(&st);
}
break;
@[end for]@
default:
printf("Unexpected topic ID to publish\n");
break;
}
}
@[end if]@
@[if recv_topics]@
bool RtpsTopics::hasMsg(char *topic_ID)
{
if (nullptr == topic_ID) return false;
*topic_ID = 0;
while (_next_sub_idx < @(len(recv_topics)) && 0 == *topic_ID)
{
switch (_sub_topics[_next_sub_idx])
{
@[for topic in recv_topics]@
case @(message_id(topic)): if (_@(topic)_sub.hasMsg()) *topic_ID = @(message_id(topic)); break;
@[end for]@
default:
printf("Unexpected topic ID to check hasMsg\n");
break;
}
_next_sub_idx++;
}
if (0 == *topic_ID)
{
_next_sub_idx = 0;
return false;
}
return true;
}
bool RtpsTopics::getMsg(const char topic_ID, eprosima::fastcdr::Cdr &scdr)
{
bool ret = false;
switch (topic_ID)
{
@[for topic in recv_topics]@
case @(message_id(topic)): // @(topic)
if (_@(topic)_sub.hasMsg())
{
@(topic)_ msg = _@(topic)_sub.getMsg();
msg.serialize(scdr);
ret = true;
}
break;
@[end for]@
default:
printf("Unexpected topic ID '%hhu' to getMsg\n", topic_ID);
break;
}
return ret;
}
@[end if]@
msg/templates/urtps/RtpsTopics.h.template
+95
View File
@@ -0,0 +1,95 @@
@###############################################
@#
@# EmPy template for generating RtpsTopics.h file
@#
@###############################################
@# Start of Template
@#
@# Context:
@# - msgs (List) list of all msg files
@###############################################
@{
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 message_id 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 <fastcdr/Cdr.h>
@[for topic in send_topics]@
#include "@(topic)_Publisher.h"
@[end for]@
@[for topic in recv_topics]@
#include "@(topic)_Subscriber.h"
@[end for]@
class RtpsTopics {
public:
bool init();
@[if send_topics]@
void publish(char topic_ID, char data_buffer[], size_t len);
@[end if]@
@[if recv_topics]@
bool hasMsg(char *topic_ID);
bool getMsg(const char topic_ID, eprosima::fastcdr::Cdr &scdr);
@[end if]@
private:
@[if send_topics]@
// Publishers
@[for topic in send_topics]@
@(topic)_Publisher _@(topic)_pub;
@[end for]@
@[end if]@
@[if recv_topics]@
// Subscribers
@[for topic in recv_topics]@
@(topic)_Subscriber _@(topic)_sub;
@[end for]@
unsigned _next_sub_idx = 0;
char _sub_topics[@(len(recv_topics))] = {
@[for topic in recv_topics]@
@(message_id(topic)), // @(topic)
@[end for]@
};
@[end if]@
};
msg/templates/urtps/Subscriber.cxx.template
+146
View File
@@ -0,0 +1,146 @@
@###############################################
@#
@# EmPy template for generating <msg>_uRTPS_UART.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/
topic = spec.short_name
}@
/****************************************************************************
*
* 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.
*
****************************************************************************/
/*!
* @@file @(topic)_Subscriber.cpp
* This file contains the implementation of the subscriber functions.
*
* This file was generated by the tool fastcdrgen.
*/
#include <fastrtps/participant/Participant.h>
#include <fastrtps/attributes/ParticipantAttributes.h>
#include <fastrtps/subscriber/Subscriber.h>
#include <fastrtps/attributes/SubscriberAttributes.h>
#include <fastrtps/Domain.h>
#include "@(topic)_Subscriber.h"
@(topic)_Subscriber::@(topic)_Subscriber() : mp_participant(nullptr), mp_subscriber(nullptr) {}
@(topic)_Subscriber::~@(topic)_Subscriber() { Domain::removeParticipant(mp_participant);}
bool @(topic)_Subscriber::init()
{
// Create RTPSParticipant
ParticipantAttributes PParam;
PParam.rtps.builtin.domainId = 0; //MUST BE THE SAME AS IN THE PUBLISHER
PParam.rtps.builtin.leaseDuration = c_TimeInfinite;
PParam.rtps.setName("Participant_subscriber"); //You can put the name you want
mp_participant = Domain::createParticipant(PParam);
if(mp_participant == nullptr)
return false;
//Register the type
Domain::registerType(mp_participant,(TopicDataType*) &myType);
// Create Subscriber
SubscriberAttributes Rparam;
Rparam.topic.topicKind = NO_KEY;
Rparam.topic.topicDataType = myType.getName(); //Must be registered before the creation of the subscriber
Rparam.topic.topicName = "@(topic)_PubSubTopic";
mp_subscriber = Domain::createSubscriber(mp_participant,Rparam,(SubscriberListener*)&m_listener);
if(mp_subscriber == nullptr)
return false;
return true;
}
void @(topic)_Subscriber::SubListener::onSubscriptionMatched(Subscriber* sub,MatchingInfo& info)
{
if (info.status == MATCHED_MATCHING)
{
n_matched++;
std::cout << "Subscriber matched" << std::endl;
}
else
{
n_matched--;
std::cout << "Subscriber unmatched" << std::endl;
}
}
void @(topic)_Subscriber::SubListener::onNewDataMessage(Subscriber* sub)
{
// Take data
if(sub->takeNextData(&msg, &m_info))
{
if(m_info.sampleKind == ALIVE)
{
// Print your structure data here.
++n_msg;
//std::cout << "Sample received, count=" << n_msg << std::endl;
has_msg = true;
}
}
}
void @(topic)_Subscriber::run()
{
std::cout << "Waiting for Data, press Enter to stop the Subscriber. "<<std::endl;
std::cin.ignore();
std::cout << "Shutting down the Subscriber." << std::endl;
}
bool @(topic)_Subscriber::hasMsg()
{
return m_listener.has_msg;
}
@(topic)_ @(topic)_Subscriber::getMsg()
{
m_listener.has_msg = false;
return m_listener.msg;
}
msg/templates/urtps/Subscriber.h.template
+99
View File
@@ -0,0 +1,99 @@
@###############################################
@#
@# EmPy template for generating <msg>_uRTPS_UART.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/
topic = spec.short_name
}@
/****************************************************************************
*
* 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.
*
****************************************************************************/
/*!
* @@file @(topic)_Subscriber.h
* This header file contains the declaration of the subscriber functions.
*
* This file was generated by the tool fastcdrgen.
*/
#ifndef _@(topic)__SUBSCRIBER_H_
#define _@(topic)__SUBSCRIBER_H_
#include <fastrtps/fastrtps_fwd.h>
#include <fastrtps/subscriber/SubscriberListener.h>
#include <fastrtps/subscriber/SampleInfo.h>
#include "@(topic)_PubSubTypes.h"
using namespace eprosima::fastrtps;
class @(topic)_Subscriber
{
public:
@(topic)_Subscriber();
virtual ~@(topic)_Subscriber();
bool init();
void run();
bool hasMsg();
@(topic)_ getMsg();
private:
Participant *mp_participant;
Subscriber *mp_subscriber;
class SubListener : public SubscriberListener
{
public:
SubListener() : n_matched(0),n_msg(0){};
~SubListener(){};
void onSubscriptionMatched(Subscriber* sub,MatchingInfo& info);
void onNewDataMessage(Subscriber* sub);
SampleInfo_t m_info;
int n_matched;
int n_msg;
@(topic)_ msg;
bool has_msg = false;
} m_listener;
@(topic)_PubSubType myType;
};
#endif // _@(topic)__SUBSCRIBER_H_
msg/templates/urtps/microRTPS_agent.cxx.template
+280
View File
@@ -0,0 +1,280 @@
@###############################################
@#
@# EmPy template for generating microRTPS_agent.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 message_id 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 <thread>
#include <unistd.h>
#include <poll.h>
#include <chrono>
#include <ctime>
#include <csignal>
#include <fastcdr/Cdr.h>
#include <fastcdr/FastCdr.h>
#include <fastcdr/exceptions/Exception.h>
#include <fastrtps/utils/eClock.h>
#include <fastrtps/Domain.h>
#include "microRTPS_transport.h"
#include "RtpsTopics.h"
#define BUFFER_SIZE 1024
// Default values
#define DEVICE "/dev/ttyACM0"
#define SLEEP_US 1
#define BAUDRATE 460800
#define POLL_MS 0
#define WAIT_CNST 2
#define DEFAULT_RECV_PORT 2020
#define DEFAULT_SEND_PORT 2019
using namespace eprosima;
using namespace eprosima::fastrtps;
volatile sig_atomic_t running = 1;
Transport_node *transport_node = nullptr;
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;
} _options;
static void usage(const char *name)
{
printf("usage: %s [options]\n\n"
" -t <transport> [UART|UDP] Default UART\n"
" -d <device> UART device. Default /dev/ttyACM0\n"
" -w <sleep_time_us> Time in us for which each iteration sleep. Default 1ms\n"
" -b <baudrate> UART device baudrate. Default 460800\n"
" -p <poll_ms> Time in ms to poll over UART. Default 1ms\n"
" -r <reception port> UDP port for receiving. Default 2019\n"
" -s <sending port> UDP port for sending. Default 2020\n",
name);
}
static int parse_options(int argc, char **argv)
{
int ch;
while ((ch = getopt(argc, argv, "t:d:w:b:p:r:s:")) != EOF)
{
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 = strtol(optarg, nullptr, 10); break;
case 'b': _options.baudrate = strtoul(optarg, nullptr, 10); break;
case 'p': _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;
default:
usage(argv[0]);
return -1;
}
}
if (optind < argc)
{
usage(argv[0]);
return -1;
}
return 0;
}
void signal_handler(int signum)
{
printf("Interrupt signal (%d) received.\n", signum);
running = 0;
transport_node->close();
}
@[if recv_topics]@
void t_send(void *data)
{
char data_buffer[BUFFER_SIZE] = {};
int length = 0;
char topic_ID = 255;
while (running)
{
// Send subscribed topics over UART
while (topics.hasMsg(&topic_ID))
{
eprosima::fastcdr::FastBuffer cdrbuffer(data_buffer, sizeof(data_buffer));
eprosima::fastcdr::Cdr scdr(cdrbuffer);
if (topics.getMsg(topic_ID, scdr))
{
length = scdr.getSerializedDataLength();
if (0 < (length = transport_node->write(topic_ID, scdr.getBufferPointer(), length)))
{
total_sent += length;
++sent;
}
}
}
usleep(_options.sleep_us);
}
}
@[end if]@
int main(int argc, char** argv)
{
if (-1 == parse_options(argc, argv))
{
printf("EXITING...\n");
return -1;
}
// register signal SIGINT and signal handler
signal(SIGINT, signal_handler);
switch (_options.transport)
{
case options::eTransports::UART:
{
transport_node = new UART_node(_options.device, _options.baudrate, _options.poll_ms);
printf("\nUART transport: device: %s; baudrate: %d; sleep: %dus; poll: %dms\n\n",
_options.device, _options.baudrate, _options.sleep_us, _options.poll_ms);
}
break;
case options::eTransports::UDP:
{
transport_node = new UDP_node(_options.recv_port, _options.send_port);
printf("\nUDP transport: recv port: %u; send port: %u; sleep: %dus\n\n",
_options.recv_port, _options.send_port, _options.sleep_us);
}
break;
default:
printf("EXITING...\n");
return -1;
}
if (0 > transport_node->init())
{
printf("EXITING...\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;
char topic_ID = 255;
std::chrono::time_point<std::chrono::steady_clock> start, end;
@[end if]@
topics.init();
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
while (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();
}
if ((receiving && std::chrono::duration<double>(std::chrono::steady_clock::now() - end).count() > WAIT_CNST) ||
(!running && loop > 1))
{
std::chrono::duration<double> elapsed_secs = end - start;
printf("\nSENT: %lu messages - %lu bytes\n",
(unsigned long)sent, (unsigned long)total_sent);
printf("RECEIVED: %d messages - %d bytes; %d LOOPS - %.03f seconds - %.02fKB/s\n\n",
received, total_read, loop, elapsed_secs.count(), (double)total_read/(1000*elapsed_secs.count()));
received = sent = total_read = total_sent = 0;
receiving = false;
}
@[end if]@
usleep(_options.sleep_us);
}
@[if recv_topics]@
sender_thread.join();
@[end if]@
delete transport_node;
transport_node = nullptr;
return 0;
}
msg/templates/urtps/microRTPS_agent_CMakeLists.txt.template
+54
View File
@@ -0,0 +1,54 @@
################################################################################
#
# 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.
#
################################################################################
cmake_minimum_required(VERSION 2.8.12)
project(micrortps_agent)
# Find requirements
find_package(fastrtps REQUIRED)
find_package(fastcdr REQUIRED)
# Set C++11
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++11 SUPPORTS_CXX11)
if(SUPPORTS_CXX11)
add_compile_options(--std=c++11)
else()
message(FATAL_ERROR "Compiler doesn't support C++11")
endif()
endif()
file(GLOB MICRORTPS_AGENT_SOURCES *.cxx)
add_executable(micrortps_agent ${MICRORTPS_AGENT_SOURCES})
target_link_libraries(micrortps_agent fastrtps fastcdr)
msg/templates/urtps/microRTPS_transport.cxx
+465
View File
@@ -0,0 +1,465 @@
/****************************************************************************
*
* 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 <unistd.h>
#include <fcntl.h>
#include <termios.h>
#include <stdio.h>
#include <errno.h>
#include <sys/socket.h>
#include "microRTPS_transport.h"
#define DEFAULT_UART "/dev/ttyACM0"
/** 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(): rx_buff_pos(0)
{
}
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(char* 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)
{
printf("Read fail %d\n", errsv);
}
return len;
}
rx_buff_pos += len;
// We read some
size_t header_size = sizeof(struct Header);
if (rx_buff_pos < header_size) return 0; //but not enough
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)
{
printf(" (↓↓ %u)\n", msg_start_pos);
// 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 = rx_buff_pos - msg_start_pos;
return -1;
}
/*
* [>,>,>,topic_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 message won't fit the buffer.
if (buffer_len < header_size + payload_len) 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)
{
printf(" (↓ %u)\n", msg_start_pos);
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)
{
printf("BAD CRC %u != %u\n", read_crc, calc_crc);
printf(" (↓ %lu)\n", (unsigned long)(header_size + payload_len));
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 -= header_size + payload_len;
memmove(rx_buffer, rx_buffer + msg_start_pos + header_size + payload_len, rx_buff_pos);
return len;
}
ssize_t Transport_node::write(const char topic_ID, char buffer[], size_t length)
{
if (!fds_OK()) return -1;
static struct Header header
{
.marker = {'>','>','>'},
.topic_ID = 0u,
.seq = 0u,
.payload_len_h = 0u,
.payload_len_l = 0u,
.crc_h = 0u,
.crc_l = 0u
};
static uint8_t seq = 0;
// [>,>,>,topic_ID,seq,payload_length,CRCHigh,CRCLow,payload_start, ... ,payload_end]
uint16_t crc = crc16((uint8_t*)buffer, length);
header.topic_ID = topic_ID;
header.seq = seq++;
header.payload_len_h = (length >> 8) & 0xff;
header.payload_len_l = length & 0xff;
header.crc_h = (crc >> 8) & 0xff;
header.crc_l = crc & 0xff;
ssize_t len = node_write(&header, sizeof(header));
if (len != sizeof(header)) goto err;
len = node_write(buffer, length);
if (len != ssize_t(length)) goto err;
return len + sizeof(header);
err:
//int errsv = errno;
//if (len == -1 ) printf(" => Writing error '%d'\n", errsv);
//else printf(" => Wrote '%ld' != length(%lu) error '%d'\n", (long)len, (unsigned long)length, errsv);
return len;
}
UART_node::UART_node(const char *_uart_name, uint32_t _baudrate, uint32_t _poll_ms):
uart_fd(-1),
baudrate(_baudrate),
poll_ms(_poll_ms)
{
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)
{
printf("failed to open device: %s (%d)\n", uart_name, errno);
return -errno;
}
// If using shared UART, no need to set it up
if (baudrate == 0) {
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;
printf("ERR GET CONF %s: %d (%d)\n", uart_name, termios_state, errno);
close();
return -errno_bkp;
}
// Clear ONLCR flag (which appends a CR for every LF)
uart_config.c_oflag &= ~ONLCR;
// USB serial is indicated by /dev/ttyACM0
if (strcmp(uart_name, "/dev/ttyACM0") != 0 && strcmp(uart_name, "/dev/ttyACM1") != 0)
{
// Set baud rate
if (cfsetispeed(&uart_config, baudrate) < 0 || cfsetospeed(&uart_config, baudrate) < 0)
{
int errno_bkp = errno;
printf("ERR SET BAUD %s: %d (%d)\n", uart_name, termios_state, errno);
close();
return -errno_bkp;
}
}
if ((termios_state = tcsetattr(uart_fd, TCSANOW, &uart_config)) < 0)
{
int errno_bkp = errno;
printf("ERR SET CONF %s (%d)\n", uart_name, errno);
close();
return -errno_bkp;
}
char aux[64];
bool flush = false;
while (0 < ::read(uart_fd, (void*)&aux, 64))
{
//printf("%s ", aux);
flush = true;
usleep(1000);
}
if (flush) printf("flush\n");
else printf("no flush\n");
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)
{
printf("Close UART\n");
::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);
}
UDP_node::UDP_node(uint16_t _udp_port_recv, uint16_t _udp_port_send):
sender_fd(-1),
receiver_fd(-1),
udp_port_recv(_udp_port_recv),
udp_port_send(_udp_port_send)
{
}
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)
{
#ifndef __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)
{
printf("create socket failed\n");
return -1;
}
printf("Trying to connect...\n");
if (bind(receiver_fd, (struct sockaddr *)&receiver_inaddr, sizeof(receiver_inaddr)) < 0)
{
printf("bind failed\n");
return -1;
}
printf("connected to server!\n");
#endif /* __PX4_NUTTX */
return 0;
}
int UDP_node::init_sender(uint16_t udp_port)
{
#ifndef __PX4_NUTTX
if ((sender_fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0)
{
printf("create socket failed\n");
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("127.0.0.1" , &sender_outaddr.sin_addr) == 0)
{
printf("inet_aton() failed\n");
return -1;
}
#endif /* __PX4_NUTTX */
return 0;
}
uint8_t UDP_node::close()
{
#ifndef __PX4_NUTTX
if (sender_fd != -1)
{
printf("Close sender socket\n");
shutdown(sender_fd, SHUT_RDWR);
::close(sender_fd);
sender_fd = -1;
}
if (receiver_fd != -1)
{
printf("Close receiver socket\n");
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;
int ret = 0;
#ifndef __PX4_NUTTX
// Blocking call
static socklen_t addrlen = sizeof(receiver_outaddr);
ret = recvfrom(receiver_fd, buffer, len, 0, (struct sockaddr*) &receiver_outaddr, &addrlen);
#endif /* __PX4_NUTTX */
return ret;
}
ssize_t UDP_node::node_write(void *buffer, size_t len)
{
if (nullptr == buffer || !fds_OK()) return -1;
int ret = 0;
#ifndef __PX4_NUTTX
ret = sendto(sender_fd, buffer, len, 0, (struct sockaddr *)&sender_outaddr, sizeof(sender_outaddr));
#endif /* __PX4_NUTTX */
return ret;
}
msg/templates/urtps/microRTPS_transport.h
+119
View File
@@ -0,0 +1,119 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
#pragma once
#include <string>
#include <cstring>
#include <arpa/inet.h>
#include <poll.h>
class Transport_node
{
public:
Transport_node();
virtual ~Transport_node();
virtual int init() {return 0;}
virtual uint8_t close() {return 0;}
ssize_t read(char* topic_ID, char out_buffer[], size_t buffer_len);
ssize_t write(const char topic_ID, char buffer[], size_t length);
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);
protected:
uint32_t rx_buff_pos;
char rx_buffer[1024] = {};
private:
struct __attribute__((packed)) Header
{
char marker[3];
uint8_t topic_ID;
uint8_t seq;
uint8_t payload_len_h;
uint8_t payload_len_l;
uint8_t crc_h;
uint8_t crc_l;
};
};
class UART_node: public Transport_node
{
public:
UART_node(const char *uart_name, uint32_t baudrate, uint32_t poll_ms);
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();
int uart_fd;
char uart_name[64] = {};
uint32_t baudrate;
uint32_t poll_ms;
struct pollfd poll_fd[1] = {};
};
class UDP_node: public Transport_node
{
public:
UDP_node(uint16_t udp_port_recv, uint16_t udp_port_send);
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;
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.template
+80
View File
@@ -0,0 +1,80 @@
@###############################################
@#
@# ROS message to IDL converter
@#
@# EmPy template for generating <msg>.idl files
@#
@################################################################################
@#
@# 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.
@#
@################################################################################
@{
import genmsg.msgs
import gencpp
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)
}@
@#################################################
@# Searching for serialize function per each field
@#################################################
@{
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 (not field.is_array):
print '\t' + str(get_idl_type_name(field.type)) + ' ' + field.name + ';'
else:
print '\t' + str(get_idl_type_name(field.base_type)) + ' ' + field.name + '[' +str(field.array_len)+ '];'
def add_msg_fields():
# 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)
for field in sorted_fields:
add_msg_field(field)
}@
@
struct @(spec.short_name)_
{
@add_msg_fields()
}; // struct @(spec.short_name)_