Vicente Monge
8 years ago
committed by
Lorenz Meier
65 changed files with 4091 additions and 8 deletions
@ -0,0 +1,191 @@
@@ -0,0 +1,191 @@
|
||||
#!/usr/bin/env python |
||||
|
||||
################################################################################ |
||||
# |
||||
# 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 sys, os, argparse, shutil |
||||
import px_generate_uorb_topic_files |
||||
import subprocess, glob |
||||
|
||||
def get_absolute_path(arg_parse_dir): |
||||
root_path = os.path.dirname(os.path.dirname(os.path.abspath(__file__))) |
||||
|
||||
if isinstance(arg_parse_dir, list): |
||||
dir = arg_parse_dir[0] |
||||
else: |
||||
dir = arg_parse_dir |
||||
|
||||
if dir[0] != '/': |
||||
dir = root_path + "/" + dir |
||||
|
||||
return dir |
||||
|
||||
|
||||
parser = argparse.ArgumentParser() |
||||
parser.add_argument("-s", "--send", dest='send', metavar='*.msg', type=str, nargs='+', help="Topics to be sended") |
||||
parser.add_argument("-r", "--receive", dest='receive', metavar='*.msg', type=str, nargs='+', help="Topics to be received") |
||||
parser.add_argument("-a", "--agent", dest='agent', action="store_true", help="Flag for generate the agent, by default is true if -c is not specified") |
||||
parser.add_argument("-c", "--client", dest='client', action="store_true", help="Flag for generate the client, by default is true if -a is not specified") |
||||
parser.add_argument("-t", "--topic-msg-dir", dest='msgdir', type=str, nargs=1, help="Topics message dir, by default msg/", default="msg") |
||||
parser.add_argument("-o", "--agent-outdir", dest='agentdir', type=str, nargs=1, help="Agent output dir, by default src/modules/micrortps_bridge/micrortps_agent", default="src/modules/micrortps_bridge/micrortps_agent") |
||||
parser.add_argument("-u", "--client-outdir", dest='clientdir', type=str, nargs=1, help="Client output dir, by default src/modules/micrortps_bridge/micrortps_client", default="src/modules/micrortps_bridge/micrortps_client") |
||||
parser.add_argument("-f", "--fastrtpsgen-dir", dest='fastrtpsgen', type=str, nargs='?', help="fastrtpsgen installation dir, only needed if fastrtpsgen is not in PATH, by default empty", default="") |
||||
parser.add_argument("--delete-tree", dest='del_tree', action="store_true", help="Delete dir tree output dir(s)") |
||||
|
||||
if len(sys.argv) <= 1: |
||||
parser.print_usage() |
||||
exit(-1) |
||||
|
||||
# Parse arguments |
||||
args = parser.parse_args() |
||||
msg_folder = get_absolute_path(args.msgdir) |
||||
msg_files_send = args.send |
||||
msg_files_receive = args.receive |
||||
agent = args.agent |
||||
client = args.client |
||||
del_tree = args.del_tree |
||||
px_generate_uorb_topic_files.append_to_include_path({msg_folder}, px_generate_uorb_topic_files.INCL_DEFAULT) |
||||
agent_out_dir = get_absolute_path(args.agentdir) |
||||
client_out_dir = get_absolute_path(args.clientdir) |
||||
|
||||
if args.fastrtpsgen is None or args.fastrtpsgen == "": |
||||
# Assume fastrtpsgen is in PATH |
||||
fastrtpsgen_path = "fastrtpsgen" |
||||
else: |
||||
# Path to fastrtpsgen is explicitly specified |
||||
fastrtpsgen_path = get_absolute_path(args.fastrtpsgen) + "/fastrtpsgen" |
||||
|
||||
# If nothing specified it's generated both |
||||
if agent == False and client == False: |
||||
agent = True |
||||
client = True |
||||
|
||||
if del_tree: |
||||
if agent: |
||||
_continue = str(raw_input("\nFiles in " + agent_out_dir + " will be erased, continue?[Y/n]\n")) |
||||
if _continue == "N" or _continue == "n": |
||||
print("Aborting execution...") |
||||
exit(-1) |
||||
else: |
||||
if agent and os.path.isdir(agent_out_dir): |
||||
shutil.rmtree(agent_out_dir) |
||||
|
||||
if client: |
||||
_continue = str(raw_input("\nFiles in " + client_out_dir + " will be erased, continue?[Y/n]\n")) |
||||
if _continue == "N" or _continue == "n": |
||||
print("Aborting execution...") |
||||
exit(-1) |
||||
else: |
||||
if client and os.path.isdir(client_out_dir): |
||||
shutil.rmtree(client_out_dir) |
||||
|
||||
if agent and os.path.isdir(agent_out_dir + "/idl"): |
||||
shutil.rmtree(agent_out_dir + "/idl") |
||||
|
||||
uorb_templates_dir = msg_folder + "/templates/uorb" |
||||
urtps_templates_dir = msg_folder + "/templates/urtps" |
||||
|
||||
uRTPS_CLIENT_TEMPL_FILE = 'microRTPS_client.cpp.template' |
||||
uRTPS_AGENT_TOPICS_H_TEMPL_FILE = 'RtpsTopics.h.template' |
||||
uRTPS_AGENT_TOPICS_SRC_TEMPL_FILE = 'RtpsTopics.cxx.template' |
||||
uRTPS_AGENT_TEMPL_FILE = 'microRTPS_agent.cxx.template' |
||||
uRTPS_AGENT_CMAKELIST_TEMPL_FILE = 'microRTPS_agent_CMakeLists.txt.template' |
||||
uRTPS_PUBLISHER_SRC_TEMPL_FILE = 'Publisher.cxx.template' |
||||
uRTPS_PUBLISHER_H_TEMPL_FILE = 'Publisher.h.template' |
||||
uRTPS_SUBSCRIBER_SRC_TEMPL_FILE = 'Subscriber.cxx.template' |
||||
uRTPS_SUBSCRIBER_H_TEMPL_FILE = 'Subscriber.h.template' |
||||
|
||||
def generate_agent(out_dir): |
||||
|
||||
if msg_files_send: |
||||
for msg_file in msg_files_send: |
||||
px_generate_uorb_topic_files.generate_idl_file(msg_file, out_dir + "/idl", urtps_templates_dir, |
||||
px_generate_uorb_topic_files.INCL_DEFAULT) |
||||
px_generate_uorb_topic_files.generate_topic_file(msg_file, out_dir, urtps_templates_dir, |
||||
px_generate_uorb_topic_files.INCL_DEFAULT, uRTPS_PUBLISHER_SRC_TEMPL_FILE) |
||||
px_generate_uorb_topic_files.generate_topic_file(msg_file, out_dir, urtps_templates_dir, |
||||
px_generate_uorb_topic_files.INCL_DEFAULT, uRTPS_PUBLISHER_H_TEMPL_FILE) |
||||
|
||||
if msg_files_receive: |
||||
for msg_file in msg_files_receive: |
||||
px_generate_uorb_topic_files.generate_idl_file(msg_file, out_dir + "/idl", urtps_templates_dir, |
||||
px_generate_uorb_topic_files.INCL_DEFAULT) |
||||
px_generate_uorb_topic_files.generate_topic_file(msg_file, out_dir, urtps_templates_dir, |
||||
px_generate_uorb_topic_files.INCL_DEFAULT, uRTPS_SUBSCRIBER_SRC_TEMPL_FILE) |
||||
px_generate_uorb_topic_files.generate_topic_file(msg_file, out_dir, urtps_templates_dir, |
||||
px_generate_uorb_topic_files.INCL_DEFAULT, uRTPS_SUBSCRIBER_H_TEMPL_FILE) |
||||
|
||||
px_generate_uorb_topic_files.generate_uRTPS_general(msg_files_send, msg_files_receive, out_dir, urtps_templates_dir, |
||||
px_generate_uorb_topic_files.INCL_DEFAULT, uRTPS_AGENT_TEMPL_FILE) |
||||
px_generate_uorb_topic_files.generate_uRTPS_general(msg_files_send, msg_files_receive, out_dir, urtps_templates_dir, |
||||
px_generate_uorb_topic_files.INCL_DEFAULT, uRTPS_AGENT_TOPICS_H_TEMPL_FILE) |
||||
px_generate_uorb_topic_files.generate_uRTPS_general(msg_files_send, msg_files_receive, out_dir, urtps_templates_dir, |
||||
px_generate_uorb_topic_files.INCL_DEFAULT, uRTPS_AGENT_TOPICS_SRC_TEMPL_FILE) |
||||
px_generate_uorb_topic_files.generate_uRTPS_general(msg_files_send, msg_files_receive, out_dir, urtps_templates_dir, |
||||
px_generate_uorb_topic_files.INCL_DEFAULT, uRTPS_AGENT_CMAKELIST_TEMPL_FILE) |
||||
|
||||
# Final steps to install agent |
||||
os.system("mkdir -p " + agent_out_dir + "/fastrtpsgen") |
||||
for idl_file in glob.glob( agent_out_dir + "/idl/*.idl"): |
||||
ret = os.system("cd " + agent_out_dir + "/fastrtpsgen && " + fastrtpsgen_path + " -example x64Linux2.6gcc " + idl_file) |
||||
if ret: |
||||
raise Exception("fastrtpsgen not found. Specify the location of fastrtpsgen with the -f flag") |
||||
|
||||
os.system("rm " + agent_out_dir + "/fastrtpsgen/*PubSubMain.cxx " |
||||
+ agent_out_dir + "/fastrtpsgen/makefile* " |
||||
+ agent_out_dir + "/fastrtpsgen/*Publisher* " |
||||
+ agent_out_dir + "/fastrtpsgen/*Subscriber*") |
||||
os.system("cp " + agent_out_dir + "/fastrtpsgen/* " + agent_out_dir) |
||||
os.system("rm -rf " + agent_out_dir + "/fastrtpsgen/") |
||||
os.system("cp " + urtps_templates_dir + "/microRTPS_transport.* " + agent_out_dir) |
||||
os.system("mv " + agent_out_dir + "/microRTPS_agent_CMakeLists.txt " + agent_out_dir + "/CMakeLists.txt") |
||||
os.system("mkdir -p " + agent_out_dir + "/build") |
||||
|
||||
return 0 |
||||
|
||||
def generate_client(out_dir): |
||||
|
||||
px_generate_uorb_topic_files.generate_uRTPS_general(msg_files_send, msg_files_receive, out_dir, uorb_templates_dir, |
||||
px_generate_uorb_topic_files.INCL_DEFAULT, uRTPS_CLIENT_TEMPL_FILE) |
||||
|
||||
# Final steps to install client |
||||
os.system("cp " + urtps_templates_dir + "/microRTPS_transport.* " + client_out_dir) |
||||
|
||||
return 0 |
||||
|
||||
if agent: |
||||
generate_agent(agent_out_dir) |
||||
print("\nAgent created in: " + agent_out_dir) |
||||
|
||||
if client: |
||||
generate_client(client_out_dir) |
||||
print("\nClient created in: " + client_out_dir) |
||||
@ -0,0 +1,108 @@
@@ -0,0 +1,108 @@
|
||||
|
||||
# Message identification constants |
||||
|
||||
|
||||
msg_id_map = { |
||||
'actuator_armed': 1, |
||||
'actuator_controls': 2, |
||||
'actuator_direct': 3, |
||||
'actuator_outputs': 4, |
||||
'adc_report': 5, |
||||
'airspeed': 6, |
||||
'att_pos_mocap': 7, |
||||
'battery_status': 8, |
||||
'camera_capture': 9, |
||||
'camera_trigger': 10, |
||||
'collision_report': 11, |
||||
'commander_state': 12, |
||||
'control_state': 13, |
||||
'cpuload': 14, |
||||
'debug_key_value': 15, |
||||
'differential_pressure': 16, |
||||
'distance_sensor': 17, |
||||
'ekf2_innovations': 18, |
||||
'ekf2_replay': 19, |
||||
'ekf2_timestamps': 20, |
||||
'esc_report': 21, |
||||
'esc_status': 22, |
||||
'estimator_status': 23, |
||||
'fence': 24, |
||||
'fence_vertex': 25, |
||||
'filtered_bottom_flow': 26, |
||||
'follow_target': 27, |
||||
'fw_pos_ctrl_status': 28, |
||||
'geofence_result': 29, |
||||
'gps_dump': 30, |
||||
'gps_inject_data': 31, |
||||
'hil_sensor': 32, |
||||
'home_position': 33, |
||||
'input_rc': 34, |
||||
'led_control': 35, |
||||
'log_message': 36, |
||||
'manual_control_setpoint': 37, |
||||
'mavlink_log': 38, |
||||
'mc_att_ctrl_status': 39, |
||||
'mission': 40, |
||||
'mission_result': 41, |
||||
'mount_orientation': 42, |
||||
'multirotor_motor_limits': 43, |
||||
'offboard_control_mode': 44, |
||||
'optical_flow': 45, |
||||
'output_pwm': 46, |
||||
'parameter_update': 47, |
||||
'position_setpoint': 48, |
||||
'position_setpoint_triplet': 49, |
||||
'pwm_input': 50, |
||||
'qshell_req': 51, |
||||
'rc_channels': 52, |
||||
'rc_parameter_map': 53, |
||||
'safety': 54, |
||||
'satellite_info': 55, |
||||
'sensor_accel': 56, |
||||
'sensor_baro': 57, |
||||
'sensor_combined': 58, |
||||
'sensor_correction': 59, |
||||
'sensor_gyro': 60, |
||||
'sensor_mag': 61, |
||||
'sensor_preflight': 62, |
||||
'sensor_selection': 63, |
||||
'servorail_status': 64, |
||||
'subsystem_info': 65, |
||||
'system_power': 66, |
||||
'task_stack_info': 67, |
||||
'tecs_status': 68, |
||||
'telemetry_status': 69, |
||||
'test_motor': 70, |
||||
'time_offset': 71, |
||||
'transponder_report': 72, |
||||
'uavcan_parameter_request': 73, |
||||
'uavcan_parameter_value': 74, |
||||
'ulog_stream_ack': 75, |
||||
'ulog_stream': 76, |
||||
'vehicle_attitude': 77, |
||||
'vehicle_attitude_setpoint': 78, |
||||
'vehicle_command_ack': 79, |
||||
'vehicle_command': 80, |
||||
'vehicle_control_mode': 81, |
||||
'vehicle_force_setpoint': 82, |
||||
'vehicle_global_position': 83, |
||||
'vehicle_global_velocity_setpoint': 84, |
||||
'vehicle_gps_position': 85, |
||||
'vehicle_land_detected': 86, |
||||
'vehicle_local_position': 87, |
||||
'vehicle_local_position_setpoint': 88, |
||||
'vehicle_rates_setpoint': 89, |
||||
'vehicle_roi': 90, |
||||
'vehicle_status_flags': 91, |
||||
'vehicle_status': 92, |
||||
'vtol_vehicle_status': 93, |
||||
'wind_estimate': 94, |
||||
} |
||||
|
||||
def message_id(message): |
||||
""" |
||||
Get id of a message |
||||
""" |
||||
if message in msg_id_map: |
||||
return msg_id_map[message] |
||||
return 0 |
||||
@ -0,0 +1,406 @@
@@ -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, µBufferWriter); |
||||
// microCDR structs for managing the microBuffer |
||||
struct microCDR microCDRWriter; |
||||
initMicroCDR(µCDRWriter, µBufferWriter); |
||||
|
||||
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, µCDRWriter); |
||||
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, ¶m); |
||||
param.sched_priority = SCHED_PRIORITY_DEFAULT; |
||||
(void)pthread_attr_setschedparam(&sender_thread_attr, ¶m); |
||||
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, µBufferReader); |
||||
// microCDR structs for managing the microBuffer |
||||
struct microCDR microCDRReader; |
||||
initMicroCDR(µCDRReader, µBufferReader); |
||||
@[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, µCDRReader); |
||||
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; |
||||
} |
||||
@ -0,0 +1,154 @@
@@ -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); |
||||
} |
||||
@ -0,0 +1,92 @@
@@ -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_ |
||||
@ -0,0 +1,159 @@
@@ -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]@ |
||||
@ -0,0 +1,95 @@
@@ -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]@ |
||||
}; |
||||
@ -0,0 +1,146 @@
@@ -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; |
||||
} |
||||
@ -0,0 +1,99 @@
@@ -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_ |
||||
@ -0,0 +1,280 @@
@@ -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; |
||||
} |
||||
@ -0,0 +1,54 @@
@@ -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) |
||||
@ -0,0 +1,465 @@
@@ -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; |
||||
} |
||||
@ -0,0 +1,119 @@
@@ -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; |
||||
}; |
||||
@ -0,0 +1,80 @@
@@ -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)_ |
||||
@ -0,0 +1,42 @@
@@ -0,0 +1,42 @@
|
||||
############################################################################ |
||||
# |
||||
# Copyright (c) 2015 PX4 Development Team. All rights reserved. |
||||
# |
||||
# Redistribution and use in source and binary forms, with or without |
||||
# modification, are permitted provided that the following conditions |
||||
# are met: |
||||
# |
||||
# 1. Redistributions of source code must retain the above copyright |
||||
# notice, this list of conditions and the following disclaimer. |
||||
# 2. Redistributions in binary form must reproduce the above copyright |
||||
# notice, this list of conditions and the following disclaimer in |
||||
# the documentation and/or other materials provided with the |
||||
# distribution. |
||||
# 3. Neither the name PX4 nor the names of its contributors may be |
||||
# used to endorse or promote products derived from this software |
||||
# without specific prior written permission. |
||||
# |
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS |
||||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED |
||||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
||||
# POSSIBILITY OF SUCH DAMAGE. |
||||
# |
||||
############################################################################ |
||||
px4_add_module( |
||||
MODULE drivers__protocol_splitter |
||||
MAIN protocol_splitter |
||||
STACK_MAIN 1200 |
||||
SRCS |
||||
protocol_splitter.cpp |
||||
DEPENDS |
||||
platforms__common |
||||
) |
||||
# vim: set noet ft=cmake fenc=utf-8 ff=unix : |
||||
@ -0,0 +1,603 @@
@@ -0,0 +1,603 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* Copyright (c) 2016 PX4 Development Team. All rights reserved. |
||||
* |
||||
* Redistribution and use in source and binary forms, with or without |
||||
* modification, are permitted provided that the following conditions |
||||
* are met: |
||||
* |
||||
* 1. Redistributions of source code must retain the above copyright |
||||
* notice, this list of conditions and the following disclaimer. |
||||
* 2. Redistributions in binary form must reproduce the above copyright |
||||
* notice, this list of conditions and the following disclaimer in |
||||
* the documentation and/or other materials provided with the |
||||
* distribution. |
||||
* 3. Neither the name PX4 nor the names of its contributors may be |
||||
* used to endorse or promote products derived from this software |
||||
* without specific prior written permission. |
||||
* |
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS |
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED |
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
||||
* POSSIBILITY OF SUCH DAMAGE. |
||||
* |
||||
****************************************************************************/ |
||||
|
||||
/**
|
||||
* @file protocol_splitter.cpp |
||||
* NuttX Driver to split mavlink 2 and another protocol on a serial port. |
||||
* Makes sure the two protocols can be read & written simultanously by 2 processes. |
||||
* It will create two devices: |
||||
* /dev/mavlink |
||||
*/ |
||||
|
||||
#include <drivers/device/device.h> |
||||
#include <px4_sem.hpp> |
||||
#include <px4_log.h> |
||||
|
||||
#include <sys/ioctl.h> |
||||
#include <unistd.h> |
||||
#include <cstdint> |
||||
|
||||
class Mavlink2Dev; |
||||
class RtpsDev; |
||||
class ReadBuffer; |
||||
|
||||
extern "C" __EXPORT int protocol_splitter_main(int argc, char *argv[]); |
||||
|
||||
struct StaticData { |
||||
Mavlink2Dev *mavlink2; |
||||
RtpsDev *rtps; |
||||
sem_t r_lock; |
||||
sem_t w_lock; |
||||
char device_name[16]; |
||||
ReadBuffer *read_buffer; |
||||
}; |
||||
|
||||
namespace |
||||
{ |
||||
static StaticData *objects = nullptr; |
||||
} |
||||
|
||||
class ReadBuffer |
||||
{ |
||||
public: |
||||
int read(int fd); |
||||
void move(void *dest, size_t pos, size_t n); |
||||
|
||||
uint8_t buffer[512] = {}; |
||||
size_t buf_size = 0; |
||||
|
||||
static const size_t BUFFER_THRESHOLD = sizeof(buffer) * 0.8; |
||||
}; |
||||
|
||||
int ReadBuffer::read(int fd) |
||||
{ |
||||
/* Discard whole buffer if it's filled beyond a threshold,
|
||||
* This should prevent buffer being filled by garbage that |
||||
* no reader (MAVLink or RTPS) can understand. |
||||
* |
||||
* TODO: a better approach would be checking if both reader |
||||
* start understanding messages beyond a certain buffer size, |
||||
* meaning that everything before is garbage. |
||||
*/ |
||||
if (buf_size > BUFFER_THRESHOLD) { |
||||
buf_size = 0; |
||||
} |
||||
|
||||
int r = ::read(fd, buffer + buf_size, sizeof(buffer) - buf_size); |
||||
if (r < 0) |
||||
return r; |
||||
|
||||
buf_size += r; |
||||
|
||||
return r; |
||||
} |
||||
|
||||
void ReadBuffer::move(void *dest, size_t pos, size_t n) |
||||
{ |
||||
ASSERT(pos < buf_size); |
||||
ASSERT(pos + n <= buf_size); |
||||
|
||||
memmove(dest, buffer + pos, n); // send desired data
|
||||
memmove(buffer + pos, buffer + (pos + n), sizeof(buffer) - pos - n); |
||||
buf_size -= n; |
||||
} |
||||
|
||||
class DevCommon : public device::CDev |
||||
{ |
||||
public: |
||||
DevCommon(const char *device_name, const char *device_path); |
||||
virtual ~DevCommon(); |
||||
|
||||
virtual int ioctl(struct file *filp, int cmd, unsigned long arg); |
||||
|
||||
virtual int open(file *filp); |
||||
virtual int close(file *filp); |
||||
|
||||
enum Operation {Read, Write}; |
||||
|
||||
protected: |
||||
|
||||
virtual pollevent_t poll_state(struct file *filp); |
||||
|
||||
|
||||
void lock(enum Operation op) |
||||
{ |
||||
sem_t *lock = op == Read ? &objects->r_lock : &objects->w_lock; |
||||
while (sem_wait(lock) != 0) { |
||||
/* The only case that an error should occur here is if
|
||||
* the wait was awakened by a signal. |
||||
*/ |
||||
ASSERT(get_errno() == EINTR); |
||||
} |
||||
} |
||||
|
||||
void unlock(enum Operation op) |
||||
{ |
||||
sem_t *lock = op == Read ? &objects->r_lock : &objects->w_lock; |
||||
sem_post(lock); |
||||
} |
||||
|
||||
int _fd = -1; |
||||
|
||||
uint16_t _packet_len; |
||||
enum class ParserState : uint8_t { |
||||
Idle = 0, |
||||
GotLength |
||||
}; |
||||
ParserState _parser_state = ParserState::Idle; |
||||
|
||||
bool _had_data = false; ///< whether poll() returned available data
|
||||
|
||||
private: |
||||
}; |
||||
|
||||
DevCommon::DevCommon(const char *device_name, const char *device_path) |
||||
: CDev(device_name, device_path) |
||||
{ |
||||
} |
||||
|
||||
DevCommon::~DevCommon() |
||||
{ |
||||
if (_fd >= 0) { |
||||
::close(_fd); |
||||
} |
||||
} |
||||
|
||||
int DevCommon::ioctl(struct file *filp, int cmd, unsigned long arg) |
||||
{ |
||||
//pretend we have enough space left to write, so mavlink will not drop data and throw off
|
||||
//our parsing state
|
||||
if (cmd == FIONSPACE) { |
||||
*(int *)arg = 1024; |
||||
return 0; |
||||
} |
||||
|
||||
return ::ioctl(_fd, cmd, arg); |
||||
} |
||||
|
||||
int DevCommon::open(file *filp) |
||||
{ |
||||
_fd = ::open(objects->device_name, O_RDWR | O_NOCTTY); |
||||
CDev::open(filp); |
||||
return _fd >= 0 ? 0 : -1; |
||||
} |
||||
|
||||
int DevCommon::close(file *filp) |
||||
{ |
||||
//int ret = ::close(_fd); // FIXME: calling this results in a dead-lock, because DevCommon::close()
|
||||
// is called from within another close(), and NuttX seems to hold a semaphore at this point
|
||||
_fd = -1; |
||||
CDev::close(filp); |
||||
return 0; |
||||
} |
||||
|
||||
pollevent_t DevCommon::poll_state(struct file *filp) |
||||
{ |
||||
pollfd fds[1]; |
||||
fds[0].fd = _fd; |
||||
fds[0].events = POLLIN; |
||||
|
||||
/* Here we should just check the poll state (which is called before an actual poll waiting).
|
||||
* Instead we poll on the fd with some timeout, and then pretend that there is data. |
||||
* This will let the calling poll return immediately (there's still no busy loop since |
||||
* we do actually poll here). |
||||
* We do this because there is no simple way with the given interface to poll on |
||||
* the _fd in here or by overriding some other method. |
||||
*/ |
||||
|
||||
int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), 100); |
||||
_had_data = ret > 0 && (fds[0].revents & POLLIN); |
||||
|
||||
return POLLIN; |
||||
} |
||||
|
||||
class Mavlink2Dev : public DevCommon |
||||
{ |
||||
public: |
||||
Mavlink2Dev(ReadBuffer *_read_buffer); |
||||
virtual ~Mavlink2Dev() {} |
||||
|
||||
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); |
||||
virtual ssize_t write(struct file *filp, const char *buffer, size_t buflen); |
||||
|
||||
protected: |
||||
ReadBuffer *_read_buffer; |
||||
size_t _remaining_partial = 0; |
||||
size_t _partial_start = 0; |
||||
uint8_t _partial_buffer[512] = {}; |
||||
}; |
||||
|
||||
Mavlink2Dev::Mavlink2Dev(ReadBuffer *read_buffer) |
||||
: DevCommon("Mavlink2", "/dev/mavlink") |
||||
, _read_buffer{read_buffer} |
||||
{ |
||||
} |
||||
|
||||
ssize_t Mavlink2Dev::read(struct file *filp, char *buffer, size_t buflen) |
||||
{ |
||||
int i, ret; |
||||
uint16_t packet_len = 0; |
||||
|
||||
/* last reading was partial (i.e., buffer didn't fit whole message),
|
||||
* so now we'll just send remaining bytes */ |
||||
if (_remaining_partial > 0) { |
||||
size_t len = _remaining_partial; |
||||
if (buflen < len) { |
||||
len = buflen; |
||||
} |
||||
memmove(buffer, _partial_buffer + _partial_start, len); |
||||
_partial_start += len; |
||||
_remaining_partial -= len; |
||||
|
||||
if (_remaining_partial == 0) { |
||||
_partial_start = 0; |
||||
} |
||||
return len; |
||||
} |
||||
|
||||
if (!_had_data) { |
||||
return 0; |
||||
} |
||||
|
||||
lock(Read); |
||||
ret = _read_buffer->read(_fd); |
||||
|
||||
if (ret < 0) |
||||
goto end; |
||||
|
||||
ret = 0; |
||||
|
||||
if (_read_buffer->buf_size < 3) |
||||
goto end; |
||||
|
||||
// Search for a mavlink packet on buffer to send it
|
||||
i = 0; |
||||
while (i < (_read_buffer->buf_size - 3) |
||||
&& _read_buffer->buffer[i] != 253 |
||||
&& _read_buffer->buffer[i] != 254) |
||||
i++; |
||||
|
||||
// We need at least the first three bytes to get packet len
|
||||
if (i >= _read_buffer->buf_size - 3) { |
||||
goto end; |
||||
} |
||||
|
||||
if (_read_buffer->buffer[i] == 253) { |
||||
uint8_t payload_len = _read_buffer->buffer[i + 1]; |
||||
uint8_t incompat_flags = _read_buffer->buffer[i + 2]; |
||||
packet_len = payload_len + 12; |
||||
|
||||
if (incompat_flags & 0x1) { //signing
|
||||
packet_len += 13; |
||||
} |
||||
} else { |
||||
packet_len = _read_buffer->buffer[i + 1] + 8; |
||||
} |
||||
|
||||
// packet is bigger than what we've read, better luck next time
|
||||
if (i + packet_len > _read_buffer->buf_size) { |
||||
goto end; |
||||
} |
||||
|
||||
/* if buffer doesn't fit message, send what's possible and copy remaining
|
||||
* data into a temporary buffer on this class */ |
||||
if (packet_len > buflen) { |
||||
_read_buffer->move(buffer, i, buflen); |
||||
_read_buffer->move(_partial_buffer, i, packet_len - buflen); |
||||
_remaining_partial = packet_len - buflen; |
||||
ret = buflen; |
||||
goto end; |
||||
} |
||||
|
||||
_read_buffer->move(buffer, i, packet_len); |
||||
ret = packet_len; |
||||
|
||||
end: |
||||
unlock(Read); |
||||
return ret; |
||||
} |
||||
|
||||
ssize_t Mavlink2Dev::write(struct file *filp, const char *buffer, size_t buflen) |
||||
{ |
||||
/*
|
||||
* we need to look into the data to make sure the output is locked for the duration |
||||
* of a whole packet. |
||||
* assumptions: |
||||
* - packet header is written all at once (or at least it contains the payload length) |
||||
* - a single write call does not contain multiple (or parts of multiple) packets |
||||
*/ |
||||
ssize_t ret = 0; |
||||
|
||||
switch (_parser_state) { |
||||
case ParserState::Idle: |
||||
ASSERT(buflen >= 3); |
||||
|
||||
if ((unsigned char)buffer[0] == 253) { |
||||
uint8_t payload_len = buffer[1]; |
||||
uint8_t incompat_flags = buffer[2]; |
||||
_packet_len = payload_len + 12; |
||||
|
||||
if (incompat_flags & 0x1) { //signing
|
||||
_packet_len += 13; |
||||
} |
||||
|
||||
_parser_state = ParserState::GotLength; |
||||
lock(Write); |
||||
|
||||
} else if ((unsigned char)buffer[0] == 254) { // mavlink 1
|
||||
uint8_t payload_len = buffer[1]; |
||||
_packet_len = payload_len + 8; |
||||
|
||||
_parser_state = ParserState::GotLength; |
||||
lock(Write); |
||||
|
||||
} else { |
||||
PX4_ERR("parser error"); |
||||
return 0; |
||||
} |
||||
|
||||
//no break
|
||||
case ParserState::GotLength: { |
||||
_packet_len -= buflen; |
||||
int buf_free; |
||||
::ioctl(_fd, FIONSPACE, (unsigned long)&buf_free); |
||||
|
||||
if (buf_free < buflen) { |
||||
//let write fail, to let mavlink know the buffer would overflow
|
||||
//(this is because in the ioctl we pretend there is always enough space)
|
||||
ret = -1; |
||||
|
||||
} else { |
||||
ret = ::write(_fd, buffer, buflen); |
||||
} |
||||
|
||||
if (_packet_len == 0) { |
||||
unlock(Write); |
||||
_parser_state = ParserState::Idle; |
||||
} |
||||
} |
||||
|
||||
break; |
||||
} |
||||
|
||||
return ret; |
||||
} |
||||
|
||||
class RtpsDev : public DevCommon |
||||
{ |
||||
public: |
||||
RtpsDev(ReadBuffer *_read_buffer); |
||||
virtual ~RtpsDev() {} |
||||
|
||||
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); |
||||
virtual ssize_t write(struct file *filp, const char *buffer, size_t buflen); |
||||
|
||||
protected: |
||||
ReadBuffer *_read_buffer; |
||||
|
||||
static const uint8_t HEADER_SIZE = 9; |
||||
}; |
||||
|
||||
RtpsDev::RtpsDev(ReadBuffer *read_buffer) |
||||
: DevCommon("Rtps", "/dev/rtps") |
||||
, _read_buffer{read_buffer} |
||||
{ |
||||
} |
||||
|
||||
ssize_t RtpsDev::read(struct file *filp, char *buffer, size_t buflen) |
||||
{ |
||||
int i, ret; |
||||
uint16_t packet_len, payload_len; |
||||
|
||||
if (!_had_data) { |
||||
return 0; |
||||
} |
||||
|
||||
lock(Read); |
||||
ret = _read_buffer->read(_fd); |
||||
|
||||
if (ret < 0) { |
||||
goto end; |
||||
} |
||||
ret = 0; |
||||
|
||||
if (_read_buffer->buf_size < HEADER_SIZE) |
||||
goto end; // starting ">>>" + topic + seq + lenhigh + lenlow + crchigh + crclow
|
||||
|
||||
// Search for a rtps packet on buffer to send it
|
||||
i = 0; |
||||
while (i < (_read_buffer->buf_size - HEADER_SIZE) && (memcmp(_read_buffer->buffer + i, ">>>", 3) != 0)) |
||||
i++; |
||||
|
||||
// We need at least the first six bytes to get packet len
|
||||
if (i >= _read_buffer->buf_size - HEADER_SIZE) { |
||||
goto end; |
||||
} |
||||
|
||||
payload_len = ((uint16_t)_read_buffer->buffer[i + 5] << 8) | _read_buffer->buffer[i + 6]; |
||||
packet_len = payload_len + HEADER_SIZE; |
||||
|
||||
// packet is bigger than what we've read, better luck next time
|
||||
if (i + packet_len > _read_buffer->buf_size) { |
||||
goto end; |
||||
} |
||||
|
||||
// buffer should be big enough to hold a rtps packet
|
||||
if (packet_len > buflen) { |
||||
ret = -EMSGSIZE; |
||||
goto end; |
||||
} |
||||
|
||||
_read_buffer->move(buffer, i, packet_len); |
||||
ret = packet_len; |
||||
|
||||
end: |
||||
unlock(Read); |
||||
return ret; |
||||
} |
||||
|
||||
ssize_t RtpsDev::write(struct file *filp, const char *buffer, size_t buflen) |
||||
{ |
||||
/*
|
||||
* we need to look into the data to make sure the output is locked for the duration |
||||
* of a whole packet. |
||||
* assumptions: |
||||
* - packet header is written all at once (or at least it contains the payload length) |
||||
* - a single write call does not contain multiple (or parts of multiple) packets |
||||
*/ |
||||
ssize_t ret = 0; |
||||
uint16_t payload_len; |
||||
|
||||
switch (_parser_state) { |
||||
case ParserState::Idle: |
||||
ASSERT(buflen >= HEADER_SIZE); |
||||
if (memcmp(buffer, ">>>", 3) != 0) { |
||||
PX4_ERR("parser error"); |
||||
return 0; |
||||
} |
||||
|
||||
payload_len = ((uint16_t)buffer[5] << 8) | buffer[6]; |
||||
_packet_len = payload_len + HEADER_SIZE; |
||||
_parser_state = ParserState::GotLength; |
||||
lock(Write); |
||||
|
||||
|
||||
//no break
|
||||
case ParserState::GotLength: { |
||||
_packet_len -= buflen; |
||||
int buf_free; |
||||
::ioctl(_fd, FIONSPACE, (unsigned long)&buf_free); |
||||
|
||||
// TODO should I care about this for rtps?
|
||||
if (buf_free < buflen) { |
||||
//let write fail, to let rtps know the buffer would overflow
|
||||
//(this is because in the ioctl we pretend there is always enough space)
|
||||
ret = -1; |
||||
|
||||
} else { |
||||
ret = ::write(_fd, buffer, buflen); |
||||
} |
||||
|
||||
if (_packet_len == 0) { |
||||
unlock(Write); |
||||
_parser_state = ParserState::Idle; |
||||
} |
||||
} |
||||
|
||||
break; |
||||
} |
||||
|
||||
return ret; |
||||
} |
||||
|
||||
int protocol_splitter_main(int argc, char *argv[]) |
||||
{ |
||||
if (argc < 2) { |
||||
goto out; |
||||
} |
||||
|
||||
/*
|
||||
* Start/load the driver. |
||||
*/ |
||||
if (!strcmp(argv[1], "start")) { |
||||
if (objects) { |
||||
PX4_ERR("already running"); |
||||
return 1; |
||||
} |
||||
|
||||
if (argc != 3) { |
||||
goto out; |
||||
} |
||||
|
||||
objects = new StaticData(); |
||||
|
||||
if (!objects) { |
||||
PX4_ERR("alloc failed"); |
||||
return -1; |
||||
} |
||||
|
||||
strncpy(objects->device_name, argv[2], sizeof(objects->device_name)); |
||||
sem_init(&objects->r_lock, 1, 1); |
||||
sem_init(&objects->w_lock, 1, 1); |
||||
objects->read_buffer = new ReadBuffer(); |
||||
objects->mavlink2 = new Mavlink2Dev(objects->read_buffer); |
||||
objects->rtps = new RtpsDev(objects->read_buffer); |
||||
|
||||
if (!objects->mavlink2 || !objects->rtps) { |
||||
delete objects->mavlink2; |
||||
delete objects->rtps; |
||||
delete objects->read_buffer; |
||||
sem_destroy(&objects->r_lock); |
||||
sem_destroy(&objects->w_lock); |
||||
delete objects; |
||||
objects = nullptr; |
||||
PX4_ERR("alloc failed"); |
||||
return -1; |
||||
|
||||
} else { |
||||
objects->mavlink2->init(); |
||||
objects->rtps->init(); |
||||
} |
||||
} |
||||
|
||||
if (!strcmp(argv[1], "stop")) { |
||||
if (objects) { |
||||
delete objects->mavlink2; |
||||
delete objects->rtps; |
||||
delete objects->read_buffer; |
||||
sem_destroy(&objects->r_lock); |
||||
sem_destroy(&objects->w_lock); |
||||
delete objects; |
||||
objects = nullptr; |
||||
} |
||||
} |
||||
|
||||
/*
|
||||
* Print driver status. |
||||
*/ |
||||
if (!strcmp(argv[1], "status")) { |
||||
if (objects) { |
||||
PX4_INFO("running"); |
||||
|
||||
} else { |
||||
PX4_INFO("not running"); |
||||
} |
||||
} |
||||
|
||||
return 0; |
||||
|
||||
out: |
||||
PX4_ERR("unrecognized command, try 'start <device>', 'stop', 'status'"); |
||||
return 1; |
||||
} |
||||
|
||||
@ -0,0 +1 @@
@@ -0,0 +1 @@
|
||||
Subproject commit f2193957eb36365680647c6ed07a8bf8a3e71a9e |
||||
@ -0,0 +1,115 @@
@@ -0,0 +1,115 @@
|
||||
# Hello world |
||||
|
||||
As a basic example we go to explain how implement a simple use case what sends information of some sensors (*sensor_combined* uORB topic) in the direction PX4-Fast RTPS and receives information as a text message (*log_message* uORB topic) sent in the other direction. |
||||
|
||||
## Creating the code |
||||
|
||||
``` sh |
||||
$ cd /path/to/PX4/Firmware |
||||
$ python Tools/generate_microRTPS_bridge.py --send msg/sensor_combined.msg --receive msg/sensor_combined.msg msg/log_message.msg -u src/examples/micrortps_client |
||||
``` |
||||
**NOTE**: It may be needed specify other different arguments, as the path to the Fast RTPS *bin* installation folder if it was installed in other path different to default one (*-f /path/to/fastrtps/installation/bin*). For more information, click this [link](README.md#generate-and-installing-the-client-and-the-agent). |
||||
|
||||
That generates and installs the PX4 side of the code (the client) in *src/examples/micrortps_client* and the Fast RPS side (the agent) in *src/modules/micrortps_bridge/micrortps_agent*. |
||||
|
||||
To see the message received in the client one time each second (**src/examples/micrortps_client/microRTPS_client.cpp**), we change the default value of the sleep to 1000 and we will add this *printf* to the code under the *orb_publish* line for *log_message* topic (line 274): |
||||
|
||||
```cpp |
||||
... |
||||
#define SLEEP_MS 1000 |
||||
... |
||||
... |
||||
... |
||||
case 36: |
||||
{ |
||||
deserialize_log_message(&log_message_data, data_buffer, µCDRReader); |
||||
if (!log_message_pub) |
||||
log_message_pub = orb_advertise(ORB_ID(log_message), &log_message_data); |
||||
else |
||||
orb_publish(ORB_ID(log_message), log_message_pub, &log_message_data); |
||||
printf("%s\n", log_message_data.text); |
||||
++received; |
||||
} |
||||
... |
||||
``` |
||||
To enable the compilation of the example client we need to modify the *micrortps_client* line in the cmake of our platform (*cmake/configs*) in that way: |
||||
|
||||
``` cmake |
||||
set(config_module_list |
||||
... |
||||
#src/modules/micrortps_bridge/micrortps_client |
||||
src/examples/micrortps_client |
||||
... |
||||
) |
||||
``` |
||||
|
||||
Also we need to a add basic CMakeLists.txt like that: |
||||
|
||||
``` cmake |
||||
px4_add_module( |
||||
MODULE examples__micrortps_client |
||||
MAIN micrortps_client |
||||
STACK_MAIN 4096 |
||||
SRCS |
||||
microRTPS_transport.cxx |
||||
microRTPS_client.cpp |
||||
DEPENDS |
||||
platforms__common |
||||
) |
||||
``` |
||||
|
||||
Now, we change the agent in order to send a log message each time we receive a **Fast RTPS message** with the info of the uORB topic *sensor_combined* (in the Fast RTPS world this topic will be called *sensor_combined_PubSubTopic*). |
||||
|
||||
- In the **src/modules/micrortps_bridge/micrortps_agent/RtpsTopic.cxx** file we will change the *RtpsTopics::getMsg* function to return a *log_message* for each *sensor_combined* with the text "*The temperature is XX.XXXXXXX celsius degrees*": |
||||
|
||||
```cpp |
||||
bool RtpsTopics::getMsg(const char topic_ID, eprosima::fastcdr::Cdr &scdr) |
||||
{ |
||||
bool ret = false; |
||||
switch (topic_ID) |
||||
{ |
||||
case 58: // sensor_combined |
||||
if (_sensor_combined_sub.hasMsg()) |
||||
{ |
||||
sensor_combined_ msg = _sensor_combined_sub.getMsg(); |
||||
log_message_ log_msg = {}; |
||||
std::string message = "The temperature is " + std::to_string(msg.baro_temp_celcius()) + " celsius degrees"; |
||||
std::memcpy(log_msg.text().data(), message.c_str(), message.length()); |
||||
log_msg.serialize(scdr); |
||||
ret = true; |
||||
} |
||||
break; |
||||
default: |
||||
printf("Unexpected topic ID to getMsg\n"); |
||||
break; |
||||
} |
||||
|
||||
return ret; |
||||
} |
||||
``` |
||||
|
||||
- In the **src/micrortps_bridge/micrortps_agent/microRTPS_agent.cxx** we will change the topic ID of the received message to the topic ID of the *log_message* (**36**) that is really the topic we are handling: |
||||
|
||||
```cpp |
||||
... |
||||
if (topics.getMsg(topic_ID, scdr)) |
||||
{ |
||||
topic_ID = 36; |
||||
length = scdr.getSerializedDataLength(); |
||||
if (0 < (length = transport_node->write(topic_ID, scdr.getBufferPointer(), length))) |
||||
{ |
||||
total_sent += length; |
||||
++sent; |
||||
} |
||||
} |
||||
... |
||||
``` |
||||
## Result |
||||
|
||||
After compiling and launching both the [client](README.md#px4-firmware-the-micro-rtps-client) and the [agent](README.md#fast-rtps-the-micro-rtps-agent) we obtain this kind of messages in the client shell window (showing the message created in the agent with info from temperature sensor in the PX4 side): |
||||
|
||||
```sh |
||||
... |
||||
The temperature is: 47.779999 celsius |
||||
... |
||||
``` |
||||
@ -0,0 +1,86 @@
@@ -0,0 +1,86 @@
|
||||
############################################################################ |
||||
# |
||||
# Copyright (c) 2015 PX4 Development Team. All rights reserved. |
||||
# |
||||
# Redistribution and use in source and binary forms, with or without |
||||
# modification, are permitted provided that the following conditions |
||||
# are met: |
||||
# |
||||
# 1. Redistributions of source code must retain the above copyright |
||||
# notice, this list of conditions and the following disclaimer. |
||||
# 2. Redistributions in binary form must reproduce the above copyright |
||||
# notice, this list of conditions and the following disclaimer in |
||||
# the documentation and/or other materials provided with the |
||||
# distribution. |
||||
# 3. Neither the name PX4 nor the names of its contributors may be |
||||
# used to endorse or promote products derived from this software |
||||
# without specific prior written permission. |
||||
# |
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS |
||||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED |
||||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
||||
# POSSIBILITY OF SUCH DAMAGE. |
||||
# |
||||
############################################################################ |
||||
|
||||
if(NOT GENERATE_RTPS_BRIDGE MATCHES "off") |
||||
include_directories(.) |
||||
|
||||
# Do not delete everything in the current dir |
||||
set(msg_out_path ${CMAKE_CURRENT_BINARY_DIR}) |
||||
set(topic_msg_path ${PX4_SOURCE_DIR}/msg) |
||||
|
||||
set(send_topic_files) |
||||
foreach(topic ${config_rtps_send_topics}) |
||||
list(APPEND send_topic_files ${PX4_SOURCE_DIR}/msg/${topic}.msg) |
||||
endforeach() |
||||
|
||||
set(receive_topic_files) |
||||
foreach(topic ${config_rtps_receive_topics}) |
||||
list(APPEND receive_topic_files ${PX4_SOURCE_DIR}/msg/${topic}.msg) |
||||
endforeach() |
||||
|
||||
set(topic_bridge_files_out microRTPS_client.cpp microRTPS_transport.cxx microRTPS_transport.h) |
||||
foreach(topic ${config_rtps_send_topics}) |
||||
list(APPEND topic_bridge_files_out ${msg_out_path}/${topic}_Publisher.cxx) |
||||
list(APPEND topic_bridge_files_out ${msg_out_path}/${topic}_Publisher.h) |
||||
endforeach() |
||||
|
||||
foreach(topic ${config_rtps_receive_topics}) |
||||
list(APPEND topic_bridge_files_out ${msg_out_path}/${topic}_Subscriber.cxx) |
||||
list(APPEND topic_bridge_files_out ${msg_out_path}/${topic}_Subscriber.h) |
||||
endforeach() |
||||
|
||||
add_custom_command(OUTPUT ${topic_bridge_files_out} |
||||
COMMAND ${PYTHON_EXECUTABLE} |
||||
${PX4_SOURCE_DIR}/Tools/generate_microRTPS_bridge.py |
||||
-f $ENV{FASTRTPSGEN_DIR} |
||||
-s ${send_topic_files} |
||||
-r ${receive_topic_files} |
||||
-t ${topic_msg_path} |
||||
-u ${msg_out_path} |
||||
DEPENDS ${DEPENDS} ${send_topic_files} ${receive_topic_files} |
||||
COMMENT "Generating RTPS topic bridge" |
||||
VERBATIM |
||||
) |
||||
endif() |
||||
|
||||
px4_add_module( |
||||
MODULE modules__micrortps_bridge__micrortps_client |
||||
MAIN micrortps_client |
||||
STACK_MAIN 4096 |
||||
SRCS |
||||
microRTPS_transport.cxx |
||||
microRTPS_client.cpp |
||||
DEPENDS |
||||
platforms__common |
||||
) |
||||
# vim: set noet ft=cmake fenc=utf-8 ff=unix : |
||||
|
After Width: | Height: | Size: 178 KiB |
@ -0,0 +1,87 @@
@@ -0,0 +1,87 @@
|
||||
# Micro RTPS Throughput Test |
||||
|
||||
This a simple test to measure the throughput of the bridge. Sending an receiving messages of the 256 bytes at the same time as fast as we can. |
||||
|
||||
We will create a new message for this test called **throughput_256.msg**, the content look like this: |
||||
|
||||
``` text |
||||
uint8[256] data |
||||
``` |
||||
We will create it next to the other *.msg* files in the folder *msg/*: |
||||
|
||||
``` sh |
||||
$ cd /path/to/PX4/Firmware/msg |
||||
$ echo uint8[256] data > throughput_256.msg |
||||
``` |
||||
Now, we register the new message adding it to the list of messages in the *msg/CMakeLists.txt* file: |
||||
|
||||
``` cmake |
||||
... |
||||
wind_estimate.msg |
||||
throughput_256.msg |
||||
) |
||||
... |
||||
``` |
||||
And giving it a topic id, adding a line in the *Tools/message_id.py* script: |
||||
|
||||
``` python |
||||
... |
||||
'wind_estimate': 94, |
||||
'throughput_256': 95, |
||||
} |
||||
... |
||||
``` |
||||
|
||||
## Creating the code |
||||
|
||||
First at all, we need to disable the automatic generation in the PX4 compiling process setting the variable `GENERATE_RTPS_BRIDGE` to *off* inside the *.cmake* file for the target platform (*cmake/configs/*): |
||||
|
||||
```sh |
||||
set(GENERATE_RTPS_BRIDGE off) |
||||
``` |
||||
|
||||
And then generate the code: |
||||
|
||||
``` sh |
||||
$ cd /path/to/PX4/Firmware |
||||
$ python Tools/generate_microRTPS_bridge.py --send msg/throughput_256.msg --receive msg/throughput_256.msg |
||||
``` |
||||
|
||||
That generates and installs the PX4 side of the code (the client) in *src/modules/micrortps_bridge/micrortps_client* and the Fast RPS side (the agent) in *src/modules/micrortps_bridge/micrortps_agent*. |
||||
|
||||
Now, we have to modify the client to send a *throughput_256* message each time since nobody publish under this topic. In the file **src/modules/micrortps_bridge/micrortps_client/microRTPS_client.cpp** inside the *send* function the while loop should look like this: |
||||
|
||||
``` cpp |
||||
... |
||||
while (!_should_exit_task) |
||||
{ |
||||
//bool updated; |
||||
//orb_check(fds[0], &updated); |
||||
//if (updated) |
||||
{ |
||||
// obtained data for the file descriptor |
||||
struct throughput_256_s data = {}; |
||||
// copy raw data into local buffer |
||||
//orb_copy(ORB_ID(throughput_256), fds[0], &data); |
||||
data.data[0] = loop%256; |
||||
serialize_throughput_256(&data, data_buffer, &length, µCDRWriter); |
||||
if (0 < (read = transport_node->write((char)95, data_buffer, length))) |
||||
{ |
||||
total_sent += read; |
||||
++sent; |
||||
} |
||||
} |
||||
|
||||
usleep(_options.sleep_ms*1000); |
||||
++loop; |
||||
} |
||||
... |
||||
``` |
||||
## Result |
||||
|
||||
After compiling and launching both the [client](README.md#px4-firmware-side-the-micro-rtps-client) and the [agent](README.md#fast-rtps-side-the-micro-rtps-agent) we obtain a measure of throughput for our particular conditions. For a Pixracer and a ordinary PC running Ubuntu 16.04 in the client shell window we obtain: |
||||
|
||||
```sh |
||||
SENT: 13255 messages in 13255 LOOPS, 3512575 bytes in 30.994 seconds - 113.33KB/s |
||||
RECEIVED: 13251 messages in 10000 LOOPS, 3511515 bytes in 30.994 seconds - 113.30KB/s |
||||
``` |
||||
Loading…
Reference in new issue