diff --git a/.gitignore b/.gitignore
index 4c35c53cbb844e65097def96aae6f66051735dd7..b0b4eeda0b71894830d7c9020105d1f95f7439b2 100644
--- a/.gitignore
+++ b/.gitignore
@@ -62,6 +62,7 @@ GTAGS
*.creator.user
*.files
*.includes
+src/modules/micrortps_bridge/micrortps_agent/
# uavcan firmware
ROMFS/px4fmu_common/uavcan/
diff --git a/.gitmodules b/.gitmodules
index 26451efda8d6b4497801091608f924e5b50f8131..5f8d8ec431130a1f0ef9fb6a13e0dcba07e01f81 100644
--- a/.gitmodules
+++ b/.gitmodules
@@ -37,3 +37,6 @@
[submodule "src/drivers/gps/devices"]
path = src/drivers/gps/devices
url = https://github.com/PX4/GpsDrivers.git
+[submodule "src/lib/micro-CDR"]
+ path = src/lib/micro-CDR
+ url = https://github.com/eProsima/micro-CDR.git
diff --git a/CMakeLists.txt b/CMakeLists.txt
index ff3a62ed0118d1770dc90fde5925ce02d916697a..356a8fd20e833cee1f95712afd9715d8404be27e 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -204,6 +204,7 @@ px4_add_git_submodule(TARGET git_mavlink PATH "mavlink/include/mavlink/v1.0")
px4_add_git_submodule(TARGET git_mavlink2 PATH "mavlink/include/mavlink/v2.0")
px4_add_git_submodule(TARGET git_nuttx PATH "NuttX")
px4_add_git_submodule(TARGET git_uavcan PATH "src/modules/uavcan/libuavcan")
+px4_add_git_submodule(TARGET git_micro_cdr PATH "src/lib/micro-CDR")
px4_create_git_hash_header()
@@ -380,8 +381,10 @@ px4_generate_messages(TARGET msg_gen
MSG_FILES ${msg_files}
OS ${OS}
INCLUDES ${msg_include_paths}
- DEPENDS git_genmsg git_gencpp prebuild_targets
+ DEPENDS git_genmsg git_gencpp git_micro_cdr prebuild_targets
)
+include_directories("${PX4_SOURCE_DIR}/src/lib/micro-CDR/include"
+ "${PX4_BINARY_DIR}/src/lib/micro-CDR/include/microcdr")
px4_generate_airframes_xml(BOARD ${BOARD})
diff --git a/README.md b/README.md
index cb8c9386feb4e4ced0e19dae966d168c63030d8e..afe88e710878174d90233ff5969b2dd77ab4892d 100644
--- a/README.md
+++ b/README.md
@@ -62,7 +62,7 @@ The PX4 Dev Team syncs up on a [weekly dev call](https://dev.px4.io/en/contribut
* Obstacle Avoidance - [Vilhjalmur Vilhjalmsson](https://github.com/vilhjalmur89)
* [Snapdragon](https://github.com/PX4/Firmware/labels/snapdragon)
* [Christoph Tobler](https://github.com/ChristophTobler)
- * [Mark Charlebois](https://github.com/mcharleb)
+ * [Mark Charlebois](https://github.com/mcharleb)
* [Intel Aero](https://github.com/PX4/Firmware/labels/intel%20aero)
* [Lucas De Marchi](https://github.com/lucasdemarchi)
* [José Roberto de Souza](https://github.com/zehortigoza)
diff --git a/Tools/generate_microRTPS_bridge.py b/Tools/generate_microRTPS_bridge.py
new file mode 100644
index 0000000000000000000000000000000000000000..ff79fe46f890e5cd11d7407adb7f0ca09da9c112
--- /dev/null
+++ b/Tools/generate_microRTPS_bridge.py
@@ -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)
diff --git a/Tools/message_id.py b/Tools/message_id.py
new file mode 100644
index 0000000000000000000000000000000000000000..75bed8accdc69322b69a632491cb4cfa68a1355f
--- /dev/null
+++ b/Tools/message_id.py
@@ -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
diff --git a/Tools/px_generate_uorb_topic_files.py b/Tools/px_generate_uorb_topic_files.py
index 311eaa38c19f60e6c2482a653fc22da86207ae75..0c8ba8c6e93827eff7910a202e717b26f0fa1ca7 100755
--- a/Tools/px_generate_uorb_topic_files.py
+++ b/Tools/px_generate_uorb_topic_files.py
@@ -42,8 +42,8 @@ import os
import shutil
import filecmp
import argparse
-
import sys
+
px4_tools_dir = os.path.dirname(os.path.abspath(__file__))
sys.path.append(px4_tools_dir + "/genmsg/src")
sys.path.append(px4_tools_dir + "/gencpp/src")
@@ -82,7 +82,12 @@ OUTPUT_FILE_EXT = ['.h', '.cpp']
INCL_DEFAULT = ['std_msgs:./msg/std_msgs']
PACKAGE = 'px4'
TOPICS_TOKEN = '# TOPICS '
+IDL_TEMPLATE_FILE = 'msg.idl.template'
+class MsgScope:
+ NONE = 0
+ SEND = 1
+ RECEIVE = 2
def get_multi_topics(filename):
"""
@@ -141,6 +146,102 @@ def generate_output_from_file(format_idx, filename, outputdir, templatedir, incl
return generate_by_template(output_file, template_file, em_globals)
+def generate_idl_file(filename_msg, outputdir, templatedir, includepath):
+ """
+ Generates an .idl from .msg file
+ """
+ em_globals = get_em_globals(filename_msg, includepath, MsgScope.NONE)
+ spec_short_name = em_globals["spec"].short_name
+
+ # Make sure output directory exists:
+ if not os.path.isdir(outputdir):
+ os.makedirs(outputdir)
+
+ template_file = os.path.join(templatedir, IDL_TEMPLATE_FILE)
+ output_file = os.path.join(outputdir, IDL_TEMPLATE_FILE.replace("msg.idl.template", str(spec_short_name + "_.idl")))
+
+ return generate_by_template(output_file, template_file, em_globals)
+
+def generate_uRTPS_general(filename_send_msgs, filename_received_msgs,
+ outputdir, templatedir, includepath, template_name):
+ """
+ Generates source file by UART msg content
+ """
+ em_globals_list = []
+ if filename_send_msgs:
+ em_globals_list.extend([get_em_globals(f, includepath, MsgScope.SEND) for f in filename_send_msgs])
+
+ if filename_received_msgs:
+ em_globals_list.extend([get_em_globals(f, includepath, MsgScope.RECEIVE) for f in filename_received_msgs])
+
+ merged_em_globals = merge_em_globals_list(em_globals_list)
+ # Make sure output directory exists:
+ if not os.path.isdir(outputdir):
+ os.makedirs(outputdir)
+
+ template_file = os.path.join(templatedir, template_name)
+ output_file = os.path.join(outputdir, template_name.replace(".template", ""))
+
+ return generate_by_template(output_file, template_file, merged_em_globals)
+
+def generate_topic_file(filename_msg, outputdir, templatedir, includepath, template_name):
+ """
+ Generates an .idl from .msg file
+ """
+ em_globals = get_em_globals(filename_msg, includepath, MsgScope.NONE)
+ spec_short_name = em_globals["spec"].short_name
+
+ # Make sure output directory exists:
+ if not os.path.isdir(outputdir):
+ os.makedirs(outputdir)
+
+ template_file = os.path.join(templatedir, template_name)
+ output_file = os.path.join(outputdir, spec_short_name + "_" + template_name.replace(".template", ""))
+
+ return generate_by_template(output_file, template_file, em_globals)
+
+def get_em_globals(filename_msg, includepath, scope):
+ """
+ Generates em globals dictionary
+ """
+ msg_context = genmsg.msg_loader.MsgContext.create_default()
+ full_type_name = genmsg.gentools.compute_full_type_name(PACKAGE, os.path.basename(filename_msg))
+ spec = genmsg.msg_loader.load_msg_from_file(msg_context, filename_msg, full_type_name)
+ topics = get_multi_topics(filename_msg)
+ if includepath:
+ search_path = genmsg.command_line.includepath_to_dict(includepath)
+ else:
+ search_path = {}
+ genmsg.msg_loader.load_depends(msg_context, spec, search_path)
+ md5sum = genmsg.gentools.compute_md5(msg_context, spec)
+ if len(topics) == 0:
+ topics.append(spec.short_name)
+ em_globals = {
+ "file_name_in": filename_msg,
+ "md5sum": md5sum,
+ "search_path": search_path,
+ "msg_context": msg_context,
+ "spec": spec,
+ "topics": topics,
+ "scope": scope
+ }
+
+ return em_globals
+
+def merge_em_globals_list(em_globals_list):
+ """
+ Merges a list of em_globals to a single dictionary where each attribute is a list
+ """
+ if len(em_globals_list) < 1:
+ return {}
+
+ merged_em_globals = {}
+ for name in em_globals_list[0]:
+ merged_em_globals[name] = [em_globals[name] for em_globals in em_globals_list]
+
+ return merged_em_globals
+
+
def generate_by_template(output_file, template_file, em_globals):
"""
@@ -151,7 +252,7 @@ def generate_by_template(output_file, template_file, em_globals):
folder_name = os.path.dirname(output_file)
if not os.path.exists(folder_name):
os.makedirs(folder_name)
-
+
ofile = open(output_file, 'w')
# todo, reuse interpreter
interpreter = em.Interpreter(output=ofile, globals=em_globals, options={em.RAW_OPT:True,em.BUFFERED_OPT:True})
@@ -268,7 +369,7 @@ def generate_topics_list_file(msgdir, outputdir, templatedir):
tl_template_file = os.path.join(templatedir, TOPICS_LIST_TEMPLATE_FILE)
tl_out_file = os.path.join(outputdir, TOPICS_LIST_TEMPLATE_FILE.replace(".template", ""))
generate_by_template(tl_out_file, tl_template_file, tl_globals)
-
+
def generate_topics_list_file_from_files(files, outputdir, templatedir):
# generate cpp file with topics list
filenames = [os.path.basename(p) for p in files if os.path.basename(p).endswith(".msg")]
diff --git a/Tools/px_generate_uorb_topic_helper.py b/Tools/px_generate_uorb_topic_helper.py
index 9a28b8a2eb1c7fca5c51fbb13c1f18f91c2f53f9..7f55b0ee6af0e7e0400c43efead3566e6d611af5 100644
--- a/Tools/px_generate_uorb_topic_helper.py
+++ b/Tools/px_generate_uorb_topic_helper.py
@@ -23,6 +23,36 @@ type_map = {
'char': 'char',
}
+type_serialize_map = {
+ 'int8': 'SignedChar',
+ 'int16': 'Short',
+ 'int32': 'Int',
+ 'int64': 'Long',
+ 'uint8': 'UnsignedChar',
+ 'uint16': 'UnsignedShort',
+ 'uint32': 'UnsignedInt',
+ 'uint64': 'UnsignedLong',
+ 'float32': 'Float',
+ 'float64': 'Double',
+ 'bool': 'Bool',
+ 'char': 'Char',
+}
+
+type_idl_map = {
+ 'int8': 'octet',
+ 'int16': 'short',
+ 'int32': 'long',
+ 'int64': 'long long',
+ 'uint8': 'octet',
+ 'uint16': 'unsigned short',
+ 'uint32': 'unsigned long',
+ 'uint64': 'unsigned long long',
+ 'float32': 'float',
+ 'float64': 'double',
+ 'bool': 'boolean',
+ 'char': 'char',
+}
+
msgtype_size_map = {
'int8': 1,
'int16': 2,
diff --git a/cmake/configs/nuttx_aerocore2_default.cmake b/cmake/configs/nuttx_aerocore2_default.cmake
index cb5daaf240463c605e56259ff87c963cf90a6f61..835b762c18c18cf45aa1035f79b7a3fe5e8ca0fc 100644
--- a/cmake/configs/nuttx_aerocore2_default.cmake
+++ b/cmake/configs/nuttx_aerocore2_default.cmake
@@ -131,6 +131,7 @@ set(config_module_list
lib/version
lib/DriverFramework/framework
platforms/nuttx
+ lib/micro-CDR
# had to add for cmake, not sure why wasn't in original config
platforms/common
diff --git a/cmake/configs/nuttx_aerofc-v1_default.cmake b/cmake/configs/nuttx_aerofc-v1_default.cmake
index af13fa6f9a7af8b92cdcc2791b350c887785cc90..446bc8ce564ca013351fa1083deadc8fd69cb911 100644
--- a/cmake/configs/nuttx_aerofc-v1_default.cmake
+++ b/cmake/configs/nuttx_aerofc-v1_default.cmake
@@ -108,6 +108,7 @@ set(config_module_list
lib/DriverFramework/framework
lib/rc
platforms/nuttx
+ lib/micro-CDR
# had to add for cmake, not sure why wasn't in original config
platforms/common
diff --git a/cmake/configs/nuttx_auav-x21_default.cmake b/cmake/configs/nuttx_auav-x21_default.cmake
index 52ded465196e3197329f9a988cb338e047e4ac7d..29a042c168830623928622f3fc478c31511fcefe 100644
--- a/cmake/configs/nuttx_auav-x21_default.cmake
+++ b/cmake/configs/nuttx_auav-x21_default.cmake
@@ -152,6 +152,7 @@ set(config_module_list
lib/version
lib/DriverFramework/framework
platforms/nuttx
+ lib/micro-CDR
# had to add for cmake, not sure why wasn't in original config
platforms/common
diff --git a/cmake/configs/nuttx_crazyflie_default.cmake b/cmake/configs/nuttx_crazyflie_default.cmake
index 3d8fcb339eb19fcfced1f331cbea9ae381a7ccb1..b23784892a6612b6e672d13453dcbbb4ab4721b2 100644
--- a/cmake/configs/nuttx_crazyflie_default.cmake
+++ b/cmake/configs/nuttx_crazyflie_default.cmake
@@ -100,6 +100,7 @@ set(config_module_list
lib/version
lib/DriverFramework/framework
platforms/nuttx
+ lib/micro-CDR
# had to add for cmake, not sure why wasn't in original config
platforms/common
diff --git a/cmake/configs/nuttx_esc35-v1_default.cmake b/cmake/configs/nuttx_esc35-v1_default.cmake
index 63eca8132e7af2486089d9626ddcd82456ae9a3f..b8a10fb25af77bd65f9f1c3e5251ef04898ed67a 100644
--- a/cmake/configs/nuttx_esc35-v1_default.cmake
+++ b/cmake/configs/nuttx_esc35-v1_default.cmake
@@ -76,6 +76,7 @@ set(config_module_list
platforms/nuttx
platforms/common
platforms/nuttx/px4_layer
+ lib/micro-CDR
)
diff --git a/cmake/configs/nuttx_mindpx-v2_default.cmake b/cmake/configs/nuttx_mindpx-v2_default.cmake
index 2ef5efb273e437fda6b84707cd9b659d0496e5bd..3fc342be42cc838e94e236a61571cb61b644768d 100644
--- a/cmake/configs/nuttx_mindpx-v2_default.cmake
+++ b/cmake/configs/nuttx_mindpx-v2_default.cmake
@@ -156,6 +156,7 @@ set(config_module_list
lib/version
lib/DriverFramework/framework
platforms/nuttx
+ lib/micro-CDR
# had to add for cmake, not sure why wasn't in original config
platforms/common
diff --git a/cmake/configs/nuttx_px4-stm32f4discovery_default.cmake b/cmake/configs/nuttx_px4-stm32f4discovery_default.cmake
index 59f352a2ebeda6d300974b73a4b3b5d10390c358..74b38b787a6acf9132832e5ffd63c9d17267b82e 100644
--- a/cmake/configs/nuttx_px4-stm32f4discovery_default.cmake
+++ b/cmake/configs/nuttx_px4-stm32f4discovery_default.cmake
@@ -48,6 +48,7 @@ set(config_module_list
lib/version
lib/DriverFramework/framework
platforms/nuttx
+ lib/micro-CDR
# had to add for cmake, not sure why wasn't in original config
platforms/common
diff --git a/cmake/configs/nuttx_px4cannode-v1_default.cmake b/cmake/configs/nuttx_px4cannode-v1_default.cmake
index 3df9a3ec90c722d5f8ba3b0940be9d0ac8e56cdf..73f476ab32bd074179d27e95f1baa9febcbc9988 100644
--- a/cmake/configs/nuttx_px4cannode-v1_default.cmake
+++ b/cmake/configs/nuttx_px4cannode-v1_default.cmake
@@ -67,6 +67,7 @@ set(config_module_list
platforms/nuttx
platforms/common
platforms/nuttx/px4_layer
+ lib/micro-CDR
)
diff --git a/cmake/configs/nuttx_px4esc-v1_default.cmake b/cmake/configs/nuttx_px4esc-v1_default.cmake
index 6c146318d2419bb2b535fedbaa4ae0182af056b6..e5414a493e4c981868daeced3f49f19e45bb19bb 100644
--- a/cmake/configs/nuttx_px4esc-v1_default.cmake
+++ b/cmake/configs/nuttx_px4esc-v1_default.cmake
@@ -76,6 +76,7 @@ set(config_module_list
platforms/common
platforms/nuttx/px4_layer
modules/uORB
+ lib/micro-CDR
)
diff --git a/cmake/configs/nuttx_px4fmu-v1_default.cmake b/cmake/configs/nuttx_px4fmu-v1_default.cmake
index b0a1cfb46d3f69bddf92c6e485039395863a97f2..a1565aeb64847fda82179ee382327fcc8e878158 100644
--- a/cmake/configs/nuttx_px4fmu-v1_default.cmake
+++ b/cmake/configs/nuttx_px4fmu-v1_default.cmake
@@ -125,6 +125,7 @@ set(config_module_list
lib/version
lib/DriverFramework/framework
platforms/nuttx
+ lib/micro-CDR
# had to add for cmake, not sure why wasn't in original config
platforms/common
diff --git a/cmake/configs/nuttx_px4fmu-v2_default.cmake b/cmake/configs/nuttx_px4fmu-v2_default.cmake
index 6e278e6cb6c3f9b0a951ff6fede589333be481bf..a72f82defec7d613da7dc4e5c95750716f25c5fe 100644
--- a/cmake/configs/nuttx_px4fmu-v2_default.cmake
+++ b/cmake/configs/nuttx_px4fmu-v2_default.cmake
@@ -155,6 +155,7 @@ set(config_module_list
lib/version
lib/DriverFramework/framework
platforms/nuttx
+ lib/micro-CDR
# had to add for cmake, not sure why wasn't in original config
platforms/common
diff --git a/cmake/configs/nuttx_px4fmu-v2_test.cmake b/cmake/configs/nuttx_px4fmu-v2_test.cmake
index e828aac4150c1e429542adb615e36f0a74fa12f6..b0671952dfb1e5a982ec2057b292d1fd340a4812 100644
--- a/cmake/configs/nuttx_px4fmu-v2_test.cmake
+++ b/cmake/configs/nuttx_px4fmu-v2_test.cmake
@@ -150,6 +150,7 @@ set(config_module_list
lib/version
lib/DriverFramework/framework
platforms/nuttx
+ lib/micro-CDR
# had to add for cmake, not sure why wasn't in original config
platforms/common
diff --git a/cmake/configs/nuttx_px4fmu-v3_default.cmake b/cmake/configs/nuttx_px4fmu-v3_default.cmake
index f13544f8db65df601ca839a5c16d47c30aa47058..bbd1b5e9078cad07ae56dfbf732c6ed6121a6b51 100644
--- a/cmake/configs/nuttx_px4fmu-v3_default.cmake
+++ b/cmake/configs/nuttx_px4fmu-v3_default.cmake
@@ -159,6 +159,7 @@ set(config_module_list
lib/tailsitter_recovery
lib/terrain_estimation
lib/version
+ lib/micro-CDR
#
# Platform
diff --git a/cmake/configs/nuttx_px4fmu-v4_default.cmake b/cmake/configs/nuttx_px4fmu-v4_default.cmake
index 72c80de329b1af8f039172c8a8a1f4d0bb7e2d37..2bf7d52bfdae9a3bf3c2674b38b09924bc278e29 100644
--- a/cmake/configs/nuttx_px4fmu-v4_default.cmake
+++ b/cmake/configs/nuttx_px4fmu-v4_default.cmake
@@ -141,6 +141,9 @@ set(config_module_list
modules/uORB
modules/dataman
+ # micro RTPS
+ modules/micrortps_bridge/micrortps_client
+
#
# Libraries
#
@@ -161,6 +164,7 @@ set(config_module_list
lib/version
lib/DriverFramework/framework
platforms/nuttx
+ lib/micro-CDR
# had to add for cmake, not sure why wasn't in original config
platforms/common
@@ -203,6 +207,17 @@ set(config_module_list
examples/ekf_att_pos_estimator
)
+set(GENERATE_RTPS_BRIDGE on)
+
+set(config_rtps_send_topics
+ sensor_combined
+ )
+
+set(config_rtps_receive_topics
+ sensor_baro
+ )
+
+
set(config_extra_builtin_cmds
serdis
sercon
diff --git a/cmake/configs/nuttx_px4fmu-v4pro_default.cmake b/cmake/configs/nuttx_px4fmu-v4pro_default.cmake
index 7b2c72f0eb84fba42bfa347af880b5c82ba0092f..49178c5556564de69308c8bc53b79e5803069a50 100644
--- a/cmake/configs/nuttx_px4fmu-v4pro_default.cmake
+++ b/cmake/configs/nuttx_px4fmu-v4pro_default.cmake
@@ -160,6 +160,7 @@ set(config_module_list
lib/tailsitter_recovery
lib/terrain_estimation
lib/version
+ lib/micro-CDR
#
# Platform
diff --git a/cmake/configs/nuttx_px4fmu-v5_default.cmake b/cmake/configs/nuttx_px4fmu-v5_default.cmake
index 0886290320ef4c32bcc8eafcdb63c989fd298100..6acfaa899e3a8fda4cce17290174a4c016eefffc 100644
--- a/cmake/configs/nuttx_px4fmu-v5_default.cmake
+++ b/cmake/configs/nuttx_px4fmu-v5_default.cmake
@@ -159,6 +159,7 @@ set(config_module_list
lib/tailsitter_recovery
lib/terrain_estimation
lib/version
+ lib/micro-CDR
#
# Platform
diff --git a/cmake/configs/nuttx_px4nucleoF767ZI-v1_default.cmake b/cmake/configs/nuttx_px4nucleoF767ZI-v1_default.cmake
index 1cfe85bc2cc05873751adf28a5f4f1ef7efb5bb6..6f2a1f45703bd0421c0f7e67b1ec128cea19d046 100644
--- a/cmake/configs/nuttx_px4nucleoF767ZI-v1_default.cmake
+++ b/cmake/configs/nuttx_px4nucleoF767ZI-v1_default.cmake
@@ -139,6 +139,7 @@ set(config_module_list
lib/version
lib/DriverFramework/framework
platforms/nuttx
+ lib/micro-CDR
# had to add for cmake, not sure why wasn't in original config
platforms/common
diff --git a/cmake/configs/nuttx_tap-v1_default.cmake b/cmake/configs/nuttx_tap-v1_default.cmake
index b53f0b331cbb4dd4cec8760de07ae96e165e11d8..2e3e24ed4012b09e456c02a8fbb82757721bd455 100644
--- a/cmake/configs/nuttx_tap-v1_default.cmake
+++ b/cmake/configs/nuttx_tap-v1_default.cmake
@@ -106,6 +106,7 @@ set(config_module_list
lib/version
lib/DriverFramework/framework
platforms/nuttx
+ lib/micro-CDR
# had to add for cmake, not sure why wasn't in original config
platforms/common
diff --git a/cmake/configs/posix_bebop_default.cmake b/cmake/configs/posix_bebop_default.cmake
index 638f88a0b397e9aa5eee7c9474af7a7eb0332dc9..23148c25f189b240e99cff1bee2bc79c5d5315bf 100644
--- a/cmake/configs/posix_bebop_default.cmake
+++ b/cmake/configs/posix_bebop_default.cmake
@@ -88,6 +88,7 @@ set(config_module_list
lib/tailsitter_recovery
lib/version
lib/DriverFramework/framework
+ lib/micro-CDR
#
# POSIX
diff --git a/cmake/configs/posix_eagle_hil.cmake b/cmake/configs/posix_eagle_hil.cmake
index fc08ae92ee2fc54bb053e52f2b2c7be817e8ac05..0d4bc2c9dd75962a25bdb17216ad4df229edfb13 100644
--- a/cmake/configs/posix_eagle_hil.cmake
+++ b/cmake/configs/posix_eagle_hil.cmake
@@ -47,6 +47,7 @@ set(config_module_list
lib/conversion
lib/version
lib/DriverFramework/framework
+ lib/micro-CDR
platforms/common
platforms/posix/px4_layer
diff --git a/cmake/configs/posix_eagle_muorb.cmake b/cmake/configs/posix_eagle_muorb.cmake
index a04d645e0ff9ba25a01460fd517f4bb62ef98bc1..dbcab7785bca2bef9ed46d1e461b3f7661619013 100644
--- a/cmake/configs/posix_eagle_muorb.cmake
+++ b/cmake/configs/posix_eagle_muorb.cmake
@@ -19,6 +19,7 @@ set(config_module_list
modules/uORB
lib/DriverFramework/framework
+ lib/micro-CDR
platforms/posix/px4_layer
platforms/posix/work_queue
diff --git a/cmake/configs/posix_ocpoc_cross.cmake b/cmake/configs/posix_ocpoc_cross.cmake
index 75ccef93b3728cce897a0aa95b60b88a06830726..3b4056cc29e8c63af95592ce4c9ad9bb569d05c6 100644
--- a/cmake/configs/posix_ocpoc_cross.cmake
+++ b/cmake/configs/posix_ocpoc_cross.cmake
@@ -93,6 +93,7 @@ set(config_module_list
lib/DriverFramework/framework
lib/rc
lib/led
+ lib/micro-CDR
#
# POSIX
diff --git a/cmake/configs/posix_rpi_common.cmake b/cmake/configs/posix_rpi_common.cmake
index 5804660d96e33f3b342157ad9cf46681701b7b5b..fa79eab85f94ee0c3f6f6d325957451743f804b1 100644
--- a/cmake/configs/posix_rpi_common.cmake
+++ b/cmake/configs/posix_rpi_common.cmake
@@ -101,6 +101,7 @@ set(config_module_list
lib/tailsitter_recovery
lib/version
lib/DriverFramework/framework
+ lib/micro-CDR
#
# POSIX
diff --git a/cmake/configs/posix_sdflight_default.cmake b/cmake/configs/posix_sdflight_default.cmake
index 3a0969929bbecb95445f46f9699b1c38a055ec60..6a0e81f5a6cc2b8588af6144068b6a838da03953 100644
--- a/cmake/configs/posix_sdflight_default.cmake
+++ b/cmake/configs/posix_sdflight_default.cmake
@@ -65,6 +65,9 @@ set(config_module_list
modules/commander
modules/navigator
+ # micro RTPS
+ modules/micrortps_bridge/micrortps_client
+
lib/controllib
lib/mathlib
lib/mathlib/math/filter
@@ -78,6 +81,7 @@ set(config_module_list
lib/tailsitter_recovery
lib/version
lib/DriverFramework/framework
+ lib/micro-CDR
platforms/common
platforms/posix/px4_layer
diff --git a/cmake/configs/posix_sdflight_legacy.cmake b/cmake/configs/posix_sdflight_legacy.cmake
index d6685e21236567346934efcaa3d3f5879277220b..8d98adfbcbd20f0344bb38b6d16a98da6839a56b 100644
--- a/cmake/configs/posix_sdflight_legacy.cmake
+++ b/cmake/configs/posix_sdflight_legacy.cmake
@@ -57,6 +57,9 @@ set(config_module_list
modules/commander
modules/navigator
+ # micro RTPS
+ modules/micrortps_bridge/micrortps_client
+
lib/controllib
lib/mathlib
lib/mathlib/math/filter
@@ -70,6 +73,7 @@ set(config_module_list
lib/tailsitter_recovery
lib/version
lib/DriverFramework/framework
+ lib/micro-CDR
platforms/common
platforms/posix/px4_layer
diff --git a/cmake/configs/posix_sitl_default.cmake b/cmake/configs/posix_sitl_default.cmake
index 77c1471d09ae7a0710a716b425477e688c22f6e3..66e7ee796ca40e90ef06b86ef92daa917305dd7f 100644
--- a/cmake/configs/posix_sitl_default.cmake
+++ b/cmake/configs/posix_sitl_default.cmake
@@ -104,6 +104,9 @@ set(config_module_list
modules/systemlib/mixer
modules/uORB
+ # micro RTPS
+ modules/micrortps_bridge/micrortps_client
+
#
# Libraries
#
@@ -171,6 +174,9 @@ set(config_module_list
# EKF
examples/ekf_att_pos_estimator
+
+ # micro-RTPS
+ lib/micro-CDR
)
set(config_extra_builtin_cmds
@@ -178,6 +184,16 @@ set(config_extra_builtin_cmds
sercon
)
+set(GENERATE_RTPS_BRIDGE on)
+
+set(config_rtps_send_topics
+ sensor_baro
+ )
+
+set(config_rtps_receive_topics
+ sensor_combined
+ )
+
# Default config_sitl_rcS_dir (posix_sitl_default), this is overwritten later
# for the config posix_sitl_efk2 and set again, explicitly, for posix_sitl_lpe,
# which are based on posix_sitl_default.
diff --git a/cmake/configs/posix_sitl_replay.cmake b/cmake/configs/posix_sitl_replay.cmake
index a1878ab068ff59d469262533d355e20d624fae4a..1ee9b97f01c496c48c0ca5568b99c07ed5e0cf73 100644
--- a/cmake/configs/posix_sitl_replay.cmake
+++ b/cmake/configs/posix_sitl_replay.cmake
@@ -28,6 +28,7 @@ set(config_module_list
lib/geo_lookup
lib/version
lib/DriverFramework/framework
+ lib/micro-CDR
)
set(config_extra_builtin_cmds
diff --git a/cmake/configs/qurt_eagle_hello.cmake b/cmake/configs/qurt_eagle_hello.cmake
index f8fd30f33b10cfeff361825b7e00c1a7339ab188..bb009f7f86ca5c9e029dda50802c8bab428bd0ab 100644
--- a/cmake/configs/qurt_eagle_hello.cmake
+++ b/cmake/configs/qurt_eagle_hello.cmake
@@ -34,6 +34,11 @@ set(config_module_list
modules/systemlib/param
modules/systemlib
modules/uORB
+
+ #
+ # Libraries
+ #
+ lib/micro-CDR
#
# QuRT port
diff --git a/cmake/configs/qurt_eagle_hil.cmake b/cmake/configs/qurt_eagle_hil.cmake
index bc9bfbc124e0fedb2404a9e505310077aa214cc4..e085aadeb3de744fe9dc203004f39ba485af6ba8 100644
--- a/cmake/configs/qurt_eagle_hil.cmake
+++ b/cmake/configs/qurt_eagle_hil.cmake
@@ -73,6 +73,7 @@ set(config_module_list
lib/controllib
lib/version
lib/DriverFramework/framework
+ lib/micro-CDR
#
# QuRT port
diff --git a/cmake/configs/qurt_eagle_muorb.cmake b/cmake/configs/qurt_eagle_muorb.cmake
index 7f34764a328b5259ec5013630ac969886fc38bad..bd8bc0fe8acd26be696200d0aa21fbd29288d174 100644
--- a/cmake/configs/qurt_eagle_muorb.cmake
+++ b/cmake/configs/qurt_eagle_muorb.cmake
@@ -45,6 +45,7 @@ set(config_module_list
lib/conversion
lib/version
lib/DriverFramework/framework
+ lib/micro-CDR
#
# QuRT port
diff --git a/cmake/configs/qurt_eagle_test.cmake b/cmake/configs/qurt_eagle_test.cmake
index c046420a6e9517956648cb89fcdf5eafbe46540a..46f979d3bc2fb6334d971da9f8f6efa9068725d4 100644
--- a/cmake/configs/qurt_eagle_test.cmake
+++ b/cmake/configs/qurt_eagle_test.cmake
@@ -43,6 +43,7 @@ set(config_module_list
lib/mathlib/math/filter
lib/conversion
lib/DriverFramework/framework
+ lib/micro-CDR
#
# QuRT port
diff --git a/cmake/configs/qurt_eagle_travis.cmake b/cmake/configs/qurt_eagle_travis.cmake
index 09a65388479ba35d3f6263a675f4c02a6793ae4b..b6f4b589934000df2f4afb02d409506fdc42a0b2 100644
--- a/cmake/configs/qurt_eagle_travis.cmake
+++ b/cmake/configs/qurt_eagle_travis.cmake
@@ -79,6 +79,7 @@ set(config_module_list
lib/tailsitter_recovery
lib/version
lib/DriverFramework/framework
+ lib/micro-CDR
#
# QuRT port
diff --git a/cmake/configs/qurt_sdflight_default.cmake b/cmake/configs/qurt_sdflight_default.cmake
index 2fbd8c2946b5ffe2fc27993b7762a990961fa524..a1b65c01f8244ad3bc46d0c5c2b3da814f085ddb 100644
--- a/cmake/configs/qurt_sdflight_default.cmake
+++ b/cmake/configs/qurt_sdflight_default.cmake
@@ -94,6 +94,7 @@ set(config_module_list
lib/rc
lib/version
lib/DriverFramework/framework
+ lib/micro-CDR
#
# QuRT port
diff --git a/cmake/configs/qurt_sdflight_legacy.cmake b/cmake/configs/qurt_sdflight_legacy.cmake
index 24e0da9e8258865c0d58a96245f39c957d2dbb87..25401b9d137c18572fcf6b59671eb39cec659ae6 100644
--- a/cmake/configs/qurt_sdflight_legacy.cmake
+++ b/cmake/configs/qurt_sdflight_legacy.cmake
@@ -93,6 +93,7 @@ set(config_module_list
lib/tailsitter_recovery
lib/version
lib/DriverFramework/framework
+ lib/micro-CDR
#
# QuRT port
diff --git a/msg/templates/uorb/microRTPS_client.cpp.template b/msg/templates/uorb/microRTPS_client.cpp.template
new file mode 100644
index 0000000000000000000000000000000000000000..ad902cfa5b934b5fc3bb6b9abff41cb9399a5abe
--- /dev/null
+++ b/msg/templates/uorb/microRTPS_client.cpp.template
@@ -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;
+}
diff --git a/msg/templates/uorb/msg.cpp.template b/msg/templates/uorb/msg.cpp.template
index 86d151a5be26cbaff24dba299e0f44db31d7f741..a6e60d9ba69c2be6910f1ae19a030f2e5a2a2aa2 100644
--- a/msg/templates/uorb/msg.cpp.template
+++ b/msg/templates/uorb/msg.cpp.template
@@ -64,9 +64,11 @@ struct_size, padding_end_size = add_padding_bytes(sorted_fields, search_path)
topic_fields = ["uint64_t timestamp"]+["%s %s" % (convert_type(field.type), field.name) for field in sorted_fields]
}@
+#include <px4_config.h>
+#include <drivers/drv_orb_dev.h>
+#include <microcdr/microCdr.h>
#include <uORB/topics/@(topic_name).h>
-
@# join all msg files in one line e.g: "float[3] position;float[3] velocity;bool armed"
@# This is used for the logger
constexpr char __orb_@(topic_name)_fields[] = "@( ";".join(topic_fields) );";
@@ -75,3 +77,99 @@ constexpr char __orb_@(topic_name)_fields[] = "@( ";".join(topic_fields) );";
ORB_DEFINE(@multi_topic, struct @uorb_struct, @(struct_size-padding_end_size),
__orb_@(topic_name)_fields);
@[end for]
+
+@#################################################
+@# Searching for serialize function per each field
+@#################################################
+@{
+
+def print_info(field):
+ print "type: ", field.type, "name: ", field.name, "base_type: ", \
+ field.base_type, "field.is_array:", ('0', '1')[field.is_array], " array_len: ", field.array_len, \
+ "is_builtin:", ('0', '1')[field.is_builtin], "is_header:", ('0', '1')[field.is_header]
+
+def print_level_info(fields):
+ for field in fields:
+ print_info(field)
+ if (not field.is_builtin):
+ print "\n"
+ children_fields = get_children_fields(field.base_type, search_path)
+ print_level_info(children_fields)
+ print "\n"
+
+def walk_through_parsed_fields():
+ print_level_info(spec.parsed_fields())
+
+def get_serialization_type_name(type_name):
+ if type_name in type_serialize_map:
+ return type_serialize_map[type_name]
+ else:
+ raise Exception("Type {0} not supported, add to type_serialize_map!".format(type_name))
+
+def add_serialize_functions(fields, scope_name):
+ for field in fields:
+ if (not field.is_header):
+ if (field.is_builtin):
+ if (not field.is_array):
+ print "\tserialize"+str(get_serialization_type_name(field.type))+"(input->"+scope_name+str(field.name)+", microCDRWriter);"
+ else:
+ print "\tserialize"+str(get_serialization_type_name(field.base_type))+"Array(input->"+scope_name+str(field.name)+", "+str(field.array_len)+", microCDRWriter);"
+ else:
+ name = field.name
+ children_fields = get_children_fields(field.base_type, search_path)
+ if (scope_name): name = scope_name + name
+ if (not field.is_array):
+ add_serialize_functions(children_fields, name + '.')
+ else:
+ for i in xrange(field.array_len):
+ add_serialize_functions(children_fields, name + ('[%d].' %i))
+
+def add_deserialize_functions(fields, scope_name):
+ for field in fields:
+ if (not field.is_header):
+ if (field.is_builtin):
+ if (not field.is_array):
+ print "\tdeserialize"+str(get_serialization_type_name(field.type))+"(&output->"+scope_name+str(field.name)+", microCDRReader);"
+ else:
+ for i in xrange(field.array_len):
+ print "\tdeserialize"+str(get_serialization_type_name(field.base_type))+"(&output->"+scope_name+str(field.name)+ str('[%d]' %i) +", microCDRReader);"
+ else:
+ name = field.name
+ children_fields = get_children_fields(field.base_type, search_path)
+ if (scope_name): name = scope_name + name
+ if (not field.is_array):
+ add_deserialize_functions(children_fields, name + '.')
+ else:
+ for i in xrange(field.array_len):
+ add_deserialize_functions(children_fields, name + ('[%d].' %i))
+
+def add_code_to_serialize():
+ # sort fields (using a stable sort) as in the declaration of the type
+ sorted_fields = sorted(spec.parsed_fields(), key=sizeof_field_type, reverse=True)
+ add_serialize_functions(sorted_fields, "")
+
+def add_code_to_deserialize():
+ # sort fields (using a stable sort) as in the declaration of the type
+ sorted_fields = sorted(spec.parsed_fields(), key=sizeof_field_type, reverse=True)
+ add_deserialize_functions(sorted_fields, "")
+}@
+
+void serialize_@(topic_name)(const struct @(uorb_struct) *input, char *output, uint32_t *length, struct microCDR *microCDRWriter)
+{
+ if (nullptr == input || nullptr == output || nullptr == length || nullptr == microCDRWriter) return;
+
+ resetStaticMicroCDRForSerialize(microCDRWriter);
+
+@add_code_to_serialize()
+
+ (*length) = microCDRWriter->m_microBuffer->m_serializedBuffer;
+}
+
+void deserialize_@(topic_name)(struct @(uorb_struct) *output, char *input, struct microCDR *microCDRReader)
+{
+ if (nullptr == output || nullptr == input || nullptr == microCDRReader) return;
+
+ resetStaticMicroCDRForDeserialize(microCDRReader);
+
+@add_code_to_deserialize()
+}
\ No newline at end of file
diff --git a/msg/templates/uorb/msg.h.template b/msg/templates/uorb/msg.h.template
index ca22c4338e0efd7b62e3aee2382a8b0346a0ffa1..b4d9d09e6588ec1366dd243e735efbd25ad96f60 100644
--- a/msg/templates/uorb/msg.h.template
+++ b/msg/templates/uorb/msg.h.template
@@ -108,6 +108,8 @@ def print_parsed_fields():
print_field_def(field)
}@
+struct microCDR;
+
#ifdef __cplusplus
@#class @(uorb_struct) {
struct __EXPORT @(uorb_struct) {
@@ -118,6 +120,7 @@ struct @(uorb_struct) {
@# timestamp at the beginning of each topic is required for logger
uint64_t timestamp; // required for logger
@print_parsed_fields()
+
#ifdef __cplusplus
@# Constants again c++-ified
@{
@@ -134,6 +137,10 @@ for constant in spec.constants:
#endif
};
+
+void serialize_@(topic_name)(const struct @(uorb_struct) *input, char *output, uint32_t *length, struct microCDR *microCDRWriter);
+void deserialize_@(topic_name)(struct @(uorb_struct) *output, char *input, struct microCDR *microCDRReader);
+
/* register this as object request broker structure */
@[for multi_topic in topics]@
ORB_DECLARE(@multi_topic);
diff --git a/msg/templates/urtps/Publisher.cxx.template b/msg/templates/urtps/Publisher.cxx.template
new file mode 100644
index 0000000000000000000000000000000000000000..77125c08dd9c84940161dfec69f74c2837c39772
--- /dev/null
+++ b/msg/templates/urtps/Publisher.cxx.template
@@ -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);
+}
diff --git a/msg/templates/urtps/Publisher.h.template b/msg/templates/urtps/Publisher.h.template
new file mode 100644
index 0000000000000000000000000000000000000000..6776562442e9719661628676a0ef8ab4feb37901
--- /dev/null
+++ b/msg/templates/urtps/Publisher.h.template
@@ -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_
\ No newline at end of file
diff --git a/msg/templates/urtps/RtpsTopics.cxx.template b/msg/templates/urtps/RtpsTopics.cxx.template
new file mode 100644
index 0000000000000000000000000000000000000000..8b433c90e3c077d3c3429deaedfe55cf8c11ede3
--- /dev/null
+++ b/msg/templates/urtps/RtpsTopics.cxx.template
@@ -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]@
diff --git a/msg/templates/urtps/RtpsTopics.h.template b/msg/templates/urtps/RtpsTopics.h.template
new file mode 100644
index 0000000000000000000000000000000000000000..5d67a975d7b242ef9360c2ace8f3643a78b55424
--- /dev/null
+++ b/msg/templates/urtps/RtpsTopics.h.template
@@ -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]@
+};
diff --git a/msg/templates/urtps/Subscriber.cxx.template b/msg/templates/urtps/Subscriber.cxx.template
new file mode 100644
index 0000000000000000000000000000000000000000..686943d93f57785f133258f4892fd4deb892332b
--- /dev/null
+++ b/msg/templates/urtps/Subscriber.cxx.template
@@ -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;
+}
\ No newline at end of file
diff --git a/msg/templates/urtps/Subscriber.h.template b/msg/templates/urtps/Subscriber.h.template
new file mode 100644
index 0000000000000000000000000000000000000000..efff2c752fbc65650bec9d54bf5d23c2302bfa7d
--- /dev/null
+++ b/msg/templates/urtps/Subscriber.h.template
@@ -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_
\ No newline at end of file
diff --git a/msg/templates/urtps/microRTPS_agent.cxx.template b/msg/templates/urtps/microRTPS_agent.cxx.template
new file mode 100644
index 0000000000000000000000000000000000000000..80e3a43a855142024d0cffe51536e2f539e4a843
--- /dev/null
+++ b/msg/templates/urtps/microRTPS_agent.cxx.template
@@ -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;
+}
diff --git a/msg/templates/urtps/microRTPS_agent_CMakeLists.txt.template b/msg/templates/urtps/microRTPS_agent_CMakeLists.txt.template
new file mode 100644
index 0000000000000000000000000000000000000000..90727dde69216889ea360b45da703e4c530484cc
--- /dev/null
+++ b/msg/templates/urtps/microRTPS_agent_CMakeLists.txt.template
@@ -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)
\ No newline at end of file
diff --git a/msg/templates/urtps/microRTPS_transport.cxx b/msg/templates/urtps/microRTPS_transport.cxx
new file mode 100644
index 0000000000000000000000000000000000000000..38a8f38869ceb0b2243ae8910b0a08512514025c
--- /dev/null
+++ b/msg/templates/urtps/microRTPS_transport.cxx
@@ -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;
+}
diff --git a/msg/templates/urtps/microRTPS_transport.h b/msg/templates/urtps/microRTPS_transport.h
new file mode 100644
index 0000000000000000000000000000000000000000..fd69d10a31cd8199dd4f07b1d2e2c9de7386c9cc
--- /dev/null
+++ b/msg/templates/urtps/microRTPS_transport.h
@@ -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;
+};
diff --git a/msg/templates/urtps/msg.idl.template b/msg/templates/urtps/msg.idl.template
new file mode 100644
index 0000000000000000000000000000000000000000..bcf93e0e9cb8fe77242af56519f0cc02e3407c15
--- /dev/null
+++ b/msg/templates/urtps/msg.idl.template
@@ -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)_
diff --git a/src/drivers/protocol_splitter/CMakeLists.txt b/src/drivers/protocol_splitter/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..9e1e972a0e576bf2810daf62821ec02a77fd8dac
--- /dev/null
+++ b/src/drivers/protocol_splitter/CMakeLists.txt
@@ -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 :
diff --git a/src/drivers/protocol_splitter/protocol_splitter.cpp b/src/drivers/protocol_splitter/protocol_splitter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..69cbefb591fbb658fb135a415442997150188cae
--- /dev/null
+++ b/src/drivers/protocol_splitter/protocol_splitter.cpp
@@ -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;
+}
+
diff --git a/src/lib/micro-CDR b/src/lib/micro-CDR
new file mode 160000
index 0000000000000000000000000000000000000000..f2193957eb36365680647c6ed07a8bf8a3e71a9e
--- /dev/null
+++ b/src/lib/micro-CDR
@@ -0,0 +1 @@
+Subproject commit f2193957eb36365680647c6ed07a8bf8a3e71a9e
diff --git a/src/modules/mavlink/CMakeLists.txt b/src/modules/mavlink/CMakeLists.txt
index 0fb220514308d3ae5021141890d4f4e2b778abc9..76d89e6b1d7ec1abad335722e88fe0be4b200be4 100644
--- a/src/modules/mavlink/CMakeLists.txt
+++ b/src/modules/mavlink/CMakeLists.txt
@@ -36,8 +36,8 @@ include_directories(${PX4_SOURCE_DIR}/mavlink/include/mavlink)
px4_add_module(
MODULE modules__mavlink
MAIN mavlink
- STACK_MAIN 1200
- STACK_MAX 1500
+ STACK_MAIN 2000
+ STACK_MAX 2500
COMPILE_FLAGS
SRCS
mavlink.c
diff --git a/src/modules/micrortps_bridge/README.md b/src/modules/micrortps_bridge/README.md
new file mode 100644
index 0000000000000000000000000000000000000000..55e4b41ae662f53d668ae80a962f31e2aff8c7ca
--- /dev/null
+++ b/src/modules/micrortps_bridge/README.md
@@ -0,0 +1,385 @@
+# RTPS/ROS2 interface: the PX4-FastRTPS bridge
+
+This bridge adds communication capabilities between a *PX4 Autopilot* and a *Fast RTPS* application, through serial ports or
+UDP sockets, using [CDR serialization](https://en.wikipedia.org/wiki/Common_Data_Representation). The goal is to provide a RTPS (Real Time Publish Subscribe Protocol) interface to PX4. This interface will also allow sharing information with the forthcoming release of ROS2 (Robot Operating System).
+
+RTPS is the underlying protocol of DDS, an standard from the OMG (Object Management Group) providing a real-time publish/subscribe middleware that is widely used in aerospace, defense and IoT applications. It has also been adopted as the middleware for the ROS2 robotics toolkit.
+
+Fast RTPS implements the latest version of the RTPS protocol and a minimum DDS API, resulting in a very lightweight implementation of the standard and full access to the RTPS fine settings.
+
+## Code generation
+
+Support for the functionality is mainly implemented within three new (automatically generated) code blocks:
+
+- *CDR serialization functions* are generated for specified uORB topics (those that are to be sent/received). For example, the following functions are generated for the *sensor_combined.msg*:
+
+ ```sh
+ void serialize_sensor_combined(const struct sensor_combined_s *input, char *output, uint32_t *length, struct microCDR *microCDRWriter);
+ void deserialize_sensor_combined(struct sensor_combined_s *output, char *input, struct microCDR *microCDRReader);
+ ```
+
+ > **Note** *CDR serialization* provides a common format for exchanging serial data between different platforms.
+
+- A *client application* that sends and receives the CDR serialized data from topics through a selected UART or UDP port.
+
+- An *agent application* that shares the CDR serialized data via (using **Fast RTPS**) to DDS service clients (e.g. ROS2). A first step in this part is the generation of a IDL file for each topic which is the translation of the corresponding MSG file (For the case of *sensor_combined* topic the **sensor_combined_.idl** file is generated from **sensor_combined.msg** file). After that *fastrtpsgen* tool is capable to generate code from the IDL file.
+
+These pieces of code are generated within the normal PX4 Firmware generation process. They can also can be generated explicitly by calling the script **Tools/generate_microRTPS_bridge.py** (see section below).
+
+### Automatically generate client and the agent code
+
+> **Note** Before continuing we need to have [installed Fast RTPS](https://dev.px4.io/en/setup/fast-rtps-installation.html).
+
+The code needed for the client, agent, and CDR serialization is automatically generated when the PX4 Firmware is compiled. The actual functions generated is based the uORB topics to send and receive. These are listed in the **.cmake** file (**cmake/configs**) for each target platform:
+
+```cmake
+set(config_rtps_send_topics
+ sensor_accel
+ sensor_baro
+ sensor_gyro
+ # Add new topic...
+ )
+
+set(config_rtps_receive_topics
+ sensor_combined
+ telemetry_status
+ wind_estimate
+ # Add new topic...
+ )
+```
+
+> **Note** Before compiling ensure that the code generation is not disabled in this way:
+```sh
+set(GENERATE_RTPS_BRIDGE off)
+```
+
+The client application will be generated in *build_OURPLATFORM/src/modules/micrortps_bridge/micrortps_client/* folder and the agent will be created in *src/modules/micrortps_bridge/micrortps_agent/* folder.
+
+> **Note** In the process of the agent generation we use a Fast RTPS tool called *fastrtpsgen*. If you haven't installed Fast RTPS in the default path you need to specify the directory of installation of *fastrtpsgen* setting the environment variable `FASTRTPSGEN_DIR` before executing *make*.
+> On Linux/Mac this is done as shown below:
+>
+> ```sh
+ export FASTRTPSGEN_DIR=/path/to/fastrtps/install/folder/bin
+ ```
+
+### Manually generate client and agent code
+
+It is also possible to generate and install the code for the client and the agent outside the normal PX4 build process using the python script
+**Tools/generate_microRTPS_bridge.py**.
+
+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:
+
+```sh
+set(GENERATE_RTPS_BRIDGE off)
+```
+
+The tool's command syntax is shown below:
+
+```sh
+$ cd /path/to/PX4/Firmware
+$ python Tools/generate_microRTPS_bridge.py -h
+usage: generate_microRTPS_bridge.py [-h] [-s *.msg [*.msg ...]]
+ [-r *.msg [*.msg ...]] [-a] [-c]
+ [-t MSGDIR] [-o AGENTDIR] [-u CLIENTDIR]
+ [-f FASTRTPSGEN]
+
+optional arguments:
+ -h, --help show this help message and exit
+ -s *.msg [*.msg ...], --send *.msg [*.msg ...]
+ Topics to be sent
+ -r *.msg [*.msg ...], --receive *.msg [*.msg ...]
+ Topics to be received
+ -a, --agent Flag for generate the agent, by default is true if -c
+ is not specified
+ -c, --client Flag for generate the client, by default is true if -a
+ is not specified
+ -t MSGDIR, --topic-msg-dir MSGDIR
+ Topics message dir, by default msg/
+ -o AGENTDIR, --agent-outdir AGENTDIR
+ Agent output dir, by default
+ src/modules/micrortps_bridge/micrortps_agent
+ -u CLIENTDIR, --client-outdir CLIENTDIR
+ Client output dir, by default
+ src/modules/micrortps_bridge/micrortps_client
+ -f FASTRTPSGEN, --fastrtpsgen-dir FASTRTPSGEN
+ fastrtpsgen installation dir, by default /bin
+ --delete-tree Delete dir tree output dir(s)
+```
+ > **Caution with --delete-tree option** so erases the content of the `CLIENTDIR` and the `AGENTDIR` before creating new files and folders.
+
+- The argument `--send/-s` means that the application from PX4 side will send these messages, and the argument `--receive/-r` specifies which messages are going to be received.
+- The output appears in `CLIENTDIR` (`-o src/modules/micrortps_bridge/micrortps_client`, by default) and in the `AGENTDIR` (`-u src/modules/micrortps_bridge/micrortps_agent`, by default).
+- If no flag `-a` or `-c` is specified, both the client and the agent will be generated and installed.
+- The `-f` option may be needed if *Fast RTPS* was not installed in the default location (`-f /path/to/fastrtps/installation/bin`).
+
+An example of use:
+
+```sh
+$ cd /path/to/PX4/Firmware
+$ python Tools/generate_microRTPS_bridge.py -s msg/sensor_baro.msg -r msg/sensor_combined.msg
+```
+
+Checking the correct generation and installation:
+
+```sh
+$ tree src/modules/micrortps_bridge/micrortps_agent
+src/modules/micrortps_bridge/micrortps_agent
+├── build
+├── CMakeLists.txt
+├── idl
+│  ├── sensor_baro_.idl
+│  └── sensor_combined_.idl
+├── microRTPS_agent.cxx
+├── microRTPS_transport.cxx
+├── microRTPS_transport.h
+├── RtpsTopics.cxx
+├── RtpsTopics.h
+├── sensor_baro_.cxx
+├── sensor_baro_.h
+├── sensor_baro_Publisher.cxx
+├── sensor_baro_Publisher.h
+├── sensor_baro_PubSubTypes.cxx
+├── sensor_baro_PubSubTypes.h
+├── sensor_combined_.cxx
+├── sensor_combined_.h
+├── sensor_combined_PubSubTypes.cxx
+├── sensor_combined_PubSubTypes.h
+├── sensor_combined_Subscriber.cxx
+└── sensor_combined_Subscriber.h
+ 2 directories, 20 files
+```
+
+```sh
+$ tree src/modules/micrortps_bridge/micrortps_client
+src/modules/micrortps_bridge/micrortps_client
+├── CMakeLists.txt
+├── microRTPS_client.cpp
+├── microRTPS_transport.cxx
+└── microRTPS_transport.h
+ 0 directories, 4 files
+```
+
+
+## PX4 Firmware side: the micro RTPS client
+
+The client application runs as both a uORB node and as a micro RTPS node. As a uORB node the agent could be subscribed to several topics as well as publish under internal uORB topics. The application receives from a internal publishers the messages, serializes the struct and writes it trough an UART
+device or UDP port selected by the user. Also will be reading from the UART device or UDP port and then publish the info
+to the internal subscribers.
+
+Steps to use the auto generated application:
+
+- Check that the lines **modules/micrortps_bridge/micrortps_client** and **lib/micro-CDR** are present in the *.cmake* config file for the target platform (*cmake/configs/*). This enables the compilation of the client along with the **PX4** firmware:
+
+ ```sh
+ set(config_module_list
+ ...
+ lib/micro-CDR
+ ...
+ # micro RTPS
+ modules/micrortps_bridge/micrortps_client
+ ...
+ )
+ ```
+ > **Note** For Nuttx platforms (e.g. *Pixracer*) the cmake files are **cmake/configs/nuttx_px4fmu-v4_default.cmake**. For the *Snapdragon Flight* platform you can find the cmake configuration here: **cmake/configs/posix_sdflight_default.cmake**.
+
+- Construct and upload the firmware executing, for example:
+
+ ```sh
+ # For NuttX/Pixhawk flight controllers:
+ $ make px4fmu-v4_default upload
+ ```
+ ```sh
+ # For Snapdragon Flight:
+ $ make eagle_default upload
+ ```
+
+After uploading the firmware, the application can be launched typing its name and passing an variable number of arguments as shown below:
+
+```sh
+> micrortps_client start|stop [options]
+ -t <transport> [UART|UDP] Default UART
+ -d <device> UART device. Default /dev/ttyACM0
+ -u <update_time_ms> Time in ms for uORB subscribed topics update. Default 0
+ -l <loops> How many iterations will this program have. -1 for infinite. Default 10000
+ -w <sleep_time_ms> Time in ms for which each iteration sleep. Default 1ms
+ -b <baudrate> UART device baudrate. Default 460800
+ -p <poll_ms> Time in ms to poll over UART. Default 1ms
+ -r <reception port> UDP port for receiving. Default 2019
+ -s <sending port> UDP port for sending. Default 2020
+```
+
+> **Note** When working with a USB-serial adapter the maximum link speed is used (the `-b` option is ignored).
+ ```sh
+ > micrortps_client start #by default -t UART -d /dev/ttyACM0 -u 0 -l 10000 -w 1 -b 460800 -p 1
+ ```
+
+<span></span>
+> **Note** If the selected UART port is busy, it's possible that the MAVLink application is already being used. If it is the case, you can stop MAVLink from NuttShell by typing:
+ ```sh
+ > mavlink stop-all
+ ```
+
+## Fast RTPS side: the Micro RTPS agent
+
+The application has several functions and possibilities of use: get the sensor data from a system that is using the *PX4
+Firmware* (obtaining the information from the selected transport: UDP or UART), publish this to a *Fast RTPS* environment
+and, in the other direction, to send through the selected transport the information of topics that are expected in the
+*PX4* side with the info even from subscribed messages from *Fast RTPS* side.
+
+> **Note** Before running the application, you must [install Fast RTPS](https://dev.px4.io/en/setup/fast-rtps-installation.html).
+
+This section explains how create *Fast RTPS* applications using the files generated by **generate_microRTPS_bridge.py** and **fastrtpsgen** (this step performed inside install script) from *Fast RTPS*.
+
+On the *Fast RTPS* side, it will be used an application running as a Fast RTPS node and as a micro RTPS node at same time. This application allow to launch RTPS publishers and subscribers that will be using the information coming from and sending to uORB topics in the PX4 side thanks to the autogenerated idl file from the original msg file. The publisher will read data from UART/UDP, deserializes it, and make a Fast RTPS message mapping the attributes from the uORB message. The subscriber simply receives the Fast RTPS messages and send in the reverse sequence to the PX4 side. The subscriber can be launched on the Raspberry Pi or in any another device connected in the same network.
+
+To create the application, compile the code:
+
+ ```sh
+ $ cd src/modules/micrortps_bridge/microRTPS_agent
+ $ mkdir build && cd build
+ $ cmake ..
+ $ make
+ ```
+
+> **Note** To cross-compile for the Qualcomm Snapdragon Flight platform see [this link](https://github.com/eProsima/PX4-FastRTPS-PoC-Snapdragon-UDP#how-to-use).
+
+To launch the publisher run:
+
+ ```text
+ $ ./micrortps_agent [options]
+ -t <transport> [UART|UDP] Default UART
+ -d <device> UART device. Default /dev/ttyACM0
+ -w <sleep_time_us> Time in us for which each iteration sleep. Default 1ms
+ -b <baudrate> UART device baudrate. Default 460800
+ -p <poll_ms> Time in ms to poll over UART. Default 1ms
+ -r <reception port> UDP port for receiving. Default 2019
+ -s <sending port> UDP port for sending. Default 2020
+ ```
+ > **Note** If we are working with a USB-serial adapter the **-b** option will be ignored and will be working at maximum speed of the link.
+ ```sh
+ $ ./micrortps_agent # by default -t UART -d /dev/ttyACM0 -w 1 -b 460800 -p 1
+ ```
+
+
+## Creating a Listener
+
+Now that we have the Client running on the flight controller and the Agent on an offboard computer, we can create an application to communicate with the flight controller through FastRTPS. The *fastrtpsgen* script allows us to quickly generate a simple application from a IDL message file. We will use it to create a Listener which subscribes to the sensor_combined topic. The Listener can be run on any computer on the same network as the Agent, but here they will be on the same computer.
+
+```sh
+$ cd /path/to/PX4/Firmware/src/modules/micrortps_bridge
+$ mkdir micrortps_listener
+$ cd micrortps_listener
+$ fastrtpsgen -example x64Linux2.6gcc ../micrortps_agent/idl/sensor_combined_.idl
+```
+
+This creates a sample subscriber, a publisher and a main-application to run them. To print out the data from the sensor_combined topic we modify the onNewDataMessage-method in sensor_combined_Subscriber.cxx:
+
+```sh
+void sensor_combined_Subscriber::SubListener::onNewDataMessage(Subscriber* sub)
+{
+ // Take data
+ sensor_combined_ st;
+
+ if(sub->takeNextData(&st, &m_info))
+ {
+ if(m_info.sampleKind == ALIVE)
+ {
+ // Print your structure data here.
+ ++n_msg;
+ std::cout << "\n\n\n\n\n\n\n\n\n\n";
+ std::cout << "Sample received, count=" << n_msg << std::endl;
+ std::cout << "=============================" << std::endl;
+ std::cout << "gyro_rad: " << st.gyro_rad().at(0);
+ std::cout << ", " << st.gyro_rad().at(1);
+ std::cout << ", " << st.gyro_rad().at(2) << std::endl;
+ std::cout << "gyro_integral_dt: " << st.gyro_integral_dt() << std::endl;
+ std::cout << "accelerometer_timestamp_relative: " << st.accelerometer_timestamp_relative() << std::endl;
+ std::cout << "accelerometer_m_s2: " << st.accelerometer_m_s2().at(0);
+ std::cout << ", " << st.accelerometer_m_s2().at(1);
+ std::cout << ", " << st.accelerometer_m_s2().at(2) << std::endl;
+ std::cout << "accelerometer_integral_dt: " << st.accelerometer_integral_dt() << std::endl;
+ std::cout << "magnetometer_timestamp_relative: " << st.magnetometer_timestamp_relative() << std::endl;
+ std::cout << "magnetometer_ga: " << st.magnetometer_ga().at(0);
+ std::cout << ", " << st.magnetometer_ga().at(1);
+ std::cout << ", " << st.magnetometer_ga().at(2) << std::endl;
+ std::cout << "baro_timestamp_relative: " << st.baro_timestamp_relative() << std::endl;
+ std::cout << "baro_alt_meter: " << st.baro_alt_meter() << std::endl;
+ std::cout << "baro_temp_celcius: " << st.baro_temp_celcius() << std::endl;
+
+ }
+ }
+}
+
+```
+
+Now build and run the Listener:
+
+```sh
+$ make -f makefile_x64Linux2.6gcc
+$ bin/*/sensor_combined_PublisherSubscriber subscriber
+```
+
+Now you should see the alititude being printed out by the Listener
+
+```sh
+Sample received, count=10119
+Received sensor_combined data
+=============================
+gyro_rad: -0.0103228, 0.0140477, 0.000319406
+gyro_integral_dt: 0.004
+accelerometer_timestamp_relative: 0
+accelerometer_m_s2: -2.82708, -6.34799, -7.41101
+accelerometer_integral_dt: 0.004
+magnetometer_timestamp_relative: -10210
+magnetometer_ga: 0.60171, 0.0405879, -0.040995
+baro_timestamp_relative: -17469
+baro_alt_meter: 368.647
+baro_temp_celcius: 43.93
+```
+
+If the Listener does not print anything, make sure the Client is running. By default the Client runs for 10000 and than stops, to run the Client continuously run
+```sh
+$ micrortps_client start -l -1
+```
+
+
+## Throughput test
+
+[Throughput test](throughput_test.md) show some real-world examples of how to use the features described in this topic.
+
+
+## Troubleshooting
+
+### Extra steps for Raspberry Pi
+
+> **Note** Normally, for UART transport it's necessary set up the UART port in the Raspberry Pi. To enable the serial port available on Raspberry Pi connector:
+
+1. Make sure the `userid` (default is pi) is a member of the `dialout` group:
+
+ ```sh
+ $ groups pi
+ $ sudo usermod -a -G dialout pi
+ ```
+
+2. You need to stop the already running on the GPIO serial console:
+
+ ```sh
+ $ sudo raspi-config
+ ```
+
+ In the menu showed go to **Interfacing options > Serial**. Select **NO for *Would you like a login shell to be accessible over serial?*. Valid and reboot.
+
+3. Check UART in kernel:
+
+ ```sh
+ $ sudo vi /boot/config.txt
+ ```
+
+And enable UART setting `enable_uart=1`.
+
+
+## Graphical example of usage
+
+This flow chart shows graphically how the bridge works. It demonstrates a bridge that sends the topic `sensor_combined` from a Pixracer to a Raspberry Pi through UART.
+
+
diff --git a/src/modules/micrortps_bridge/hello_world.md b/src/modules/micrortps_bridge/hello_world.md
new file mode 100644
index 0000000000000000000000000000000000000000..62b3ed17cb81e82a1277db9db05ec5c72ac47307
--- /dev/null
+++ b/src/modules/micrortps_bridge/hello_world.md
@@ -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
+ ...
+```
diff --git a/src/modules/micrortps_bridge/micrortps_client/CMakeLists.txt b/src/modules/micrortps_bridge/micrortps_client/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..e9ed67d7b3ef26de9b274a02780f663a9a34cbce
--- /dev/null
+++ b/src/modules/micrortps_bridge/micrortps_client/CMakeLists.txt
@@ -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 :
diff --git a/src/modules/micrortps_bridge/res/basic_example_flow.png b/src/modules/micrortps_bridge/res/basic_example_flow.png
new file mode 100644
index 0000000000000000000000000000000000000000..a48972b7addbc2894d75590df54080d53277ea68
Binary files /dev/null and b/src/modules/micrortps_bridge/res/basic_example_flow.png differ
diff --git a/src/modules/micrortps_bridge/throughput_test.md b/src/modules/micrortps_bridge/throughput_test.md
new file mode 100644
index 0000000000000000000000000000000000000000..778bd6ccae3e785da52692d49814edd46d9a1423
--- /dev/null
+++ b/src/modules/micrortps_bridge/throughput_test.md
@@ -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
+ ```