dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Fixing rebase conflicts

This commit is contained in:
Vicente Monge 2017-05-16 17:30:56 +02:00 committed by Lorenz Meier
parent d69c53827f
commit c85039e413
65 changed files with 4091 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
.gitignore vendored
View File

@ -62,6 +62,7 @@ GTAGS
*.creator.user
*.files
*.includes
src/modules/micrortps_bridge/micrortps_agent/
# uavcan firmware
ROMFS/px4fmu_common/uavcan/

3
.gitmodules vendored
View File

@ -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

5
CMakeLists.txt
View File

@ -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})

2
README.md
View File

@ -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)

191
Tools/generate_microRTPS_bridge.py Normal file
View File

@ -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)

108
Tools/message_id.py Normal file
View File

@ -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

107
Tools/px_generate_uorb_topic_files.py
View File

@ -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")]

30
Tools/px_generate_uorb_topic_helper.py
View File

@ -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,

1
cmake/configs/nuttx_aerocore2_default.cmake
View File

@ -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

1
cmake/configs/nuttx_aerofc-v1_default.cmake
View File

@ -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

1
cmake/configs/nuttx_auav-x21_default.cmake
View File

@ -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

1
cmake/configs/nuttx_crazyflie_default.cmake
View File

@ -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

1
cmake/configs/nuttx_esc35-v1_default.cmake
View File

@ -76,6 +76,7 @@ set(config_module_list
platforms/nuttx
platforms/common
platforms/nuttx/px4_layer
lib/micro-CDR
)

1
cmake/configs/nuttx_mindpx-v2_default.cmake
View File

@ -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

1
cmake/configs/nuttx_px4-stm32f4discovery_default.cmake
View File

@ -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

1
cmake/configs/nuttx_px4cannode-v1_default.cmake
View File

@ -67,6 +67,7 @@ set(config_module_list
platforms/nuttx
platforms/common
platforms/nuttx/px4_layer
lib/micro-CDR
)

1
cmake/configs/nuttx_px4esc-v1_default.cmake
View File

@ -76,6 +76,7 @@ set(config_module_list
platforms/common
platforms/nuttx/px4_layer
modules/uORB
lib/micro-CDR
)

1
cmake/configs/nuttx_px4fmu-v1_default.cmake
View File

@ -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

1
cmake/configs/nuttx_px4fmu-v2_default.cmake
View File

@ -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

1
cmake/configs/nuttx_px4fmu-v2_test.cmake
View File

@ -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

1
cmake/configs/nuttx_px4fmu-v3_default.cmake
View File

@ -159,6 +159,7 @@ set(config_module_list
lib/tailsitter_recovery
lib/terrain_estimation
lib/version
lib/micro-CDR
#
# Platform

15
cmake/configs/nuttx_px4fmu-v4_default.cmake
View File

@ -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

1
cmake/configs/nuttx_px4fmu-v4pro_default.cmake
View File

@ -160,6 +160,7 @@ set(config_module_list
lib/tailsitter_recovery
lib/terrain_estimation
lib/version
lib/micro-CDR
#
# Platform

1
cmake/configs/nuttx_px4fmu-v5_default.cmake
View File

@ -159,6 +159,7 @@ set(config_module_list
lib/tailsitter_recovery
lib/terrain_estimation
lib/version
lib/micro-CDR
#
# Platform

1
cmake/configs/nuttx_px4nucleoF767ZI-v1_default.cmake
View File

@ -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

1
cmake/configs/nuttx_tap-v1_default.cmake
View File

@ -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

1
cmake/configs/posix_bebop_default.cmake
View File

@ -88,6 +88,7 @@ set(config_module_list
lib/tailsitter_recovery
lib/version
lib/DriverFramework/framework
lib/micro-CDR
#
# POSIX

1
cmake/configs/posix_eagle_hil.cmake
View File

@ -47,6 +47,7 @@ set(config_module_list
lib/conversion
lib/version
lib/DriverFramework/framework
lib/micro-CDR
platforms/common
platforms/posix/px4_layer

1
cmake/configs/posix_eagle_muorb.cmake
View File

@ -19,6 +19,7 @@ set(config_module_list
modules/uORB
lib/DriverFramework/framework
lib/micro-CDR
platforms/posix/px4_layer
platforms/posix/work_queue

1
cmake/configs/posix_ocpoc_cross.cmake
View File

@ -93,6 +93,7 @@ set(config_module_list
lib/DriverFramework/framework
lib/rc
lib/led
lib/micro-CDR
#
# POSIX

1
cmake/configs/posix_rpi_common.cmake
View File

@ -101,6 +101,7 @@ set(config_module_list
lib/tailsitter_recovery
lib/version
lib/DriverFramework/framework
lib/micro-CDR
#
# POSIX

4
cmake/configs/posix_sdflight_default.cmake
View File

@ -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

4
cmake/configs/posix_sdflight_legacy.cmake
View File

@ -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

16
cmake/configs/posix_sitl_default.cmake
View File

@ -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.

1
cmake/configs/posix_sitl_replay.cmake
View File

@ -28,6 +28,7 @@ set(config_module_list
lib/geo_lookup
lib/version
lib/DriverFramework/framework
lib/micro-CDR
)
set(config_extra_builtin_cmds

5
cmake/configs/qurt_eagle_hello.cmake
View File

@ -34,6 +34,11 @@ set(config_module_list
modules/systemlib/param
modules/systemlib
modules/uORB
#
# Libraries
#
lib/micro-CDR
#
# QuRT port

1
cmake/configs/qurt_eagle_hil.cmake
View File

@ -73,6 +73,7 @@ set(config_module_list
lib/controllib
lib/version
lib/DriverFramework/framework
lib/micro-CDR
#
# QuRT port

1
cmake/configs/qurt_eagle_muorb.cmake
View File

@ -45,6 +45,7 @@ set(config_module_list
lib/conversion
lib/version
lib/DriverFramework/framework
lib/micro-CDR
#
# QuRT port

1
cmake/configs/qurt_eagle_test.cmake
View File

@ -43,6 +43,7 @@ set(config_module_list
lib/mathlib/math/filter
lib/conversion
lib/DriverFramework/framework
lib/micro-CDR
#
# QuRT port

1
cmake/configs/qurt_eagle_travis.cmake
View File

@ -79,6 +79,7 @@ set(config_module_list
lib/tailsitter_recovery
lib/version
lib/DriverFramework/framework
lib/micro-CDR
#
# QuRT port

1
cmake/configs/qurt_sdflight_default.cmake
View File

@ -94,6 +94,7 @@ set(config_module_list
lib/rc
lib/version
lib/DriverFramework/framework
lib/micro-CDR
#
# QuRT port

1
cmake/configs/qurt_sdflight_legacy.cmake
View File

@ -93,6 +93,7 @@ set(config_module_list
lib/tailsitter_recovery
lib/version
lib/DriverFramework/framework
lib/micro-CDR
#
# QuRT port

406
msg/templates/uorb/microRTPS_client.cpp.template Normal file
View File

@ -0,0 +1,406 @@
@###############################################
@#
@# EmPy template for generating microRTPS_client.cpp file
@#
@###############################################
@# Start of Template
@#
@# Context:
@# - msgs (List) list of all msg files
@# - multi_topics (List) list of all multi-topic names
@###############################################
@{
import genmsg.msgs
import gencpp
from px_generate_uorb_topic_helper import * # this is in Tools/
from px_generate_uorb_topic_files import MsgScope # this is in Tools/
from message_id import * # this is in Tools/
topic_names = [single_spec.short_name for single_spec in spec]
send_topics = [s.short_name for idx, s in enumerate(spec) if scope[idx] == MsgScope.SEND]
recv_topics = [s.short_name for idx, s in enumerate(spec) if scope[idx] == MsgScope.RECEIVE]
}@
/****************************************************************************
*
* Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima).
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* 3. Neither the name of the copyright holder nor the names of its contributors
* may be used to endorse or promote products derived from this software without
* specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include <px4_config.h>
#include <px4_getopt.h>
#include <px4_tasks.h>
#include <px4_posix.h>
#include <unistd.h>
#include <stdio.h>
#include <string.h>
#include <termios.h>
#include <ctime>
#include <pthread.h>
#include <microcdr/microCdr.h>
#include <uORB/uORB.h>
#include "microRTPS_transport.h"
@[for topic in list(set(topic_names))]@
#include <uORB/topics/@(topic).h>
@[end for]@
#define BUFFER_SIZE 1024
#define UPDATE_TIME_MS 0
#define LOOPS 10000
#define SLEEP_MS 1
#define BAUDRATE 460800
#define DEVICE "/dev/ttyACM0"
#define POLL_MS 1
#define DEFAULT_RECV_PORT 2019
#define DEFAULT_SEND_PORT 2020
extern "C" __EXPORT int micrortps_client_main(int argc, char *argv[]);
void* send(void *data);
struct options {
enum class eTransports
{
UART,
UDP
};
eTransports transport = options::eTransports::UART;
char device[64] = DEVICE;
int update_time_ms = UPDATE_TIME_MS;
int loops = LOOPS;
int sleep_ms = SLEEP_MS;
uint32_t baudrate = BAUDRATE;
int poll_ms = POLL_MS;
uint16_t recv_port = DEFAULT_RECV_PORT;
uint16_t send_port = DEFAULT_SEND_PORT;
} _options;
static int _rtps_task = -1;
static bool _should_exit_task = false;
Transport_node *transport_node = nullptr;
static void usage(const char *name)
{
PX4_INFO("usage: %s start|stop [options]\n\n"
" -t <transport> [UART|UDP] Default UART\n"
" -d <device> UART device. Default /dev/ttyACM0\n"
" -u <update_time_ms> Time in ms for uORB subscribed topics update. Default 0\n"
" -l <loops> How many iterations will this program have. -1 for infinite. Default 10000\n"
" -w <sleep_time_ms> Time in ms for which each iteration sleep. Default 1ms\n"
" -b <baudrate> UART device baudrate. Default 460800\n"
" -p <poll_ms> Time in ms to poll over UART. Default 1ms\n"
" -r <reception port> UDP port for receiving. Default 2019\n"
" -s <sending port> UDP port for sending. Default 2020\n",
name);
}
static int parse_options(int argc, char *argv[])
{
int ch;
int myoptind = 1;
const char *myoptarg = nullptr;
while ((ch = px4_getopt(argc, argv, "t:d:u:l:w:b:p:r:s:", &myoptind, &myoptarg)) != EOF)
{
switch (ch)
{
case 't': _options.transport = strcmp(myoptarg, "UDP") == 0?
options::eTransports::UDP
:options::eTransports::UART; break;
case 'd': if (nullptr != myoptarg) strcpy(_options.device, myoptarg); break;
case 'u': _options.update_time_ms = strtol(myoptarg, nullptr, 10); break;
case 'l': _options.loops = strtol(myoptarg, nullptr, 10); break;
case 'w': _options.sleep_ms = strtol(myoptarg, nullptr, 10); break;
case 'b': _options.baudrate = strtoul(optarg, nullptr, 10); break;
case 'p': _options.poll_ms = strtol(optarg, nullptr, 10); break;
case 'r': _options.recv_port = strtoul(optarg, nullptr, 10); break;
case 's': _options.send_port = strtoul(optarg, nullptr, 10); break;
default:
usage(argv[1]);
return -1;
}
}
return 0;
}
@[if send_topics]@
void* send(void*)
{
char data_buffer[BUFFER_SIZE] = {};
uint64_t sent = 0, total_sent = 0;
int loop = 0, read = 0;
uint32_t length = 0;
/* subscribe to topics */
int fds[@(len(send_topics))] = {};
// orb_set_interval statblish an update interval period in milliseconds.
@[for idx, topic in enumerate(send_topics)]@
fds[@(idx)] = orb_subscribe(ORB_ID(@(topic)));
orb_set_interval(fds[@(idx)], _options.update_time_ms);
@[end for]@
// microBuffer to serialized using the user defined buffer
struct microBuffer microBufferWriter;
initStaticAlignedBuffer(data_buffer, BUFFER_SIZE, &microBufferWriter);
// microCDR structs for managing the microBuffer
struct microCDR microCDRWriter;
initMicroCDR(&microCDRWriter, &microBufferWriter);
struct timespec begin;
clock_gettime(0, &begin);
while (!_should_exit_task)
{
bool updated;
@[for idx, topic in enumerate(send_topics)]@
orb_check(fds[@(idx)], &updated);
if (updated)
{
// obtained data for the file descriptor
struct @(topic)_s data = {};
// copy raw data into local buffer
orb_copy(ORB_ID(@(topic)), fds[@(idx)], &data);
serialize_@(topic)(&data, data_buffer, &length, &microCDRWriter);
if (0 < (read = transport_node->write((char)@(message_id(topic)), data_buffer, length)))
{
total_sent += read;
++sent;
}
}
@[end for]@
usleep(_options.sleep_ms*1000);
++loop;
}
struct timespec end;
clock_gettime(0, &end);
double elapsed_secs = double(end.tv_sec - begin.tv_sec) + double(end.tv_nsec - begin.tv_nsec)/double(1000000000);
printf("\nSENT: %lu messages in %d LOOPS, %llu bytes in %.03f seconds - %.02fKB/s\n",
(unsigned long)sent, loop, total_sent, elapsed_secs, (double)total_sent/(1000*elapsed_secs));
return nullptr;
}
static int launch_send_thread(pthread_t &sender_thread)
{
pthread_attr_t sender_thread_attr;
pthread_attr_init(&sender_thread_attr);
pthread_attr_setstacksize(&sender_thread_attr, PX4_STACK_ADJUSTED(4000));
struct sched_param param;
(void)pthread_attr_getschedparam(&sender_thread_attr, &param);
param.sched_priority = SCHED_PRIORITY_DEFAULT;
(void)pthread_attr_setschedparam(&sender_thread_attr, &param);
pthread_create(&sender_thread, &sender_thread_attr, send, nullptr);
pthread_attr_destroy(&sender_thread_attr);
return 0;
}
@[end if]@
static int micrortps_start(int argc, char *argv[])
{
if (0 > parse_options(argc, argv))
{
printf("EXITING...\n");
_rtps_task = -1;
return -1;
}
switch (_options.transport)
{
case options::eTransports::UART:
{
transport_node = new UART_node(_options.device, _options.baudrate, _options.poll_ms);
printf("\nUART transport: device: %s; baudrate: %d; sleep: %dms; poll: %dms\n\n",
_options.device, _options.baudrate, _options.sleep_ms, _options.poll_ms);
}
break;
case options::eTransports::UDP:
{
transport_node = new UDP_node(_options.recv_port, _options.send_port);
printf("\nUDP transport: recv port: %u; send port: %u; sleep: %dms\n\n",
_options.recv_port, _options.send_port, _options.sleep_ms);
}
break;
default:
_rtps_task = -1;
printf("EXITING...\n");
return -1;
}
if (0 > transport_node->init())
{
printf("EXITING...\n");
_rtps_task = -1;
return -1;
}
sleep(1);
@[if recv_topics]@
char data_buffer[BUFFER_SIZE] = {};
int read = 0;
char topic_ID = 255;
// Declare received topics
@[for topic in recv_topics]@
struct @(topic)_s @(topic)_data;
orb_advert_t @(topic)_pub = nullptr;
@[end for]@
// microBuffer to deserialized using the user defined buffer
struct microBuffer microBufferReader;
initDeserializedAlignedBuffer(data_buffer, BUFFER_SIZE, &microBufferReader);
// microCDR structs for managing the microBuffer
struct microCDR microCDRReader;
initMicroCDR(&microCDRReader, &microBufferReader);
@[end if]@
int total_read = 0;
uint32_t received = 0;
struct timespec begin;
clock_gettime(0, &begin);
int loop = 0;
_should_exit_task = false;
@[if send_topics]@
// create a thread for sending data to the simulator
pthread_t sender_thread;
launch_send_thread(sender_thread);
@[end if]@
while (!_should_exit_task)
{
@[if recv_topics]@
while (0 < (read = transport_node->read(&topic_ID, data_buffer, BUFFER_SIZE)))
{
total_read += read;
switch (topic_ID)
{
@[for topic in recv_topics]@
case @(message_id(topic)):
{
deserialize_@(topic)(&@(topic)_data, data_buffer, &microCDRReader);
if (!@(topic)_pub)
@(topic)_pub = orb_advertise(ORB_ID(@(topic)), &@(topic)_data);
else
orb_publish(ORB_ID(@(topic)), @(topic)_pub, &@(topic)_data);
++received;
}
break;
@[end for]@
default:
printf("Unexpected topic ID\n");
break;
}
}
@[end if]@
// loop forever if informed loop number is negative
if (_options.loops > 0 && loop >= _options.loops) break;
usleep(_options.sleep_ms*1000);
++loop;
}
@[if send_topics]@
_should_exit_task = true;
pthread_join(sender_thread, 0);
@[end if]@
struct timespec end;
clock_gettime(0, &end);
double elapsed_secs = double(end.tv_sec - begin.tv_sec) + double(end.tv_nsec - begin.tv_nsec)/double(1000000000);
printf("RECEIVED: %lu messages in %d LOOPS, %d bytes in %.03f seconds - %.02fKB/s\n\n",
(unsigned long)received, loop, total_read, elapsed_secs, (double)total_read/(1000*elapsed_secs));
delete transport_node;
transport_node = nullptr;
PX4_INFO("exiting");
fflush(stdout);
_rtps_task = -1;
return 0;
}
int micrortps_client_main(int argc, char *argv[])
{
if (argc < 2)
{
usage(argv[0]);
return -1;
}
if (!strcmp(argv[1], "start"))
{
if (_rtps_task != -1)
{
PX4_INFO("Already running");
return -1;
}
_rtps_task = px4_task_spawn_cmd("rtps",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
4096,
(px4_main_t) micrortps_start,
(char *const *)argv);
if (_rtps_task < 0)
{
PX4_WARN("Could not start task");
_rtps_task = -1;
return -1;
}
return 0;
}
if (!strcmp(argv[1], "stop"))
{
if (_rtps_task == -1)
{
PX4_INFO("Not running");
return -1;
}
_should_exit_task = true;
if (nullptr != transport_node) transport_node->close();
return 0;
}
usage(argv[0]);
return -1;
}

100
msg/templates/uorb/msg.cpp.template
View File

@ -64,9 +64,11 @@ struct_size, padding_end_size = add_padding_bytes(sorted_fields, search_path)
topic_fields = ["uint64_t timestamp"]+["%s %s" % (convert_type(field.type), field.name) for field in sorted_fields]
}@
#include <px4_config.h>
#include <drivers/drv_orb_dev.h>
#include <microcdr/microCdr.h>
#include <uORB/topics/@(topic_name).h>
@# join all msg files in one line e.g: "float[3] position;float[3] velocity;bool armed"
@# This is used for the logger
constexpr char __orb_@(topic_name)_fields[] = "@( ";".join(topic_fields) );";
@ -75,3 +77,99 @@ constexpr char __orb_@(topic_name)_fields[] = "@( ";".join(topic_fields) );";
ORB_DEFINE(@multi_topic, struct @uorb_struct, @(struct_size-padding_end_size),
__orb_@(topic_name)_fields);
@[end for]
@#################################################
@# Searching for serialize function per each field
@#################################################
@{
def print_info(field):
print "type: ", field.type, "name: ", field.name, "base_type: ", \
field.base_type, "field.is_array:", ('0', '1')[field.is_array], " array_len: ", field.array_len, \
"is_builtin:", ('0', '1')[field.is_builtin], "is_header:", ('0', '1')[field.is_header]
def print_level_info(fields):
for field in fields:
print_info(field)
if (not field.is_builtin):
print "\n"
children_fields = get_children_fields(field.base_type, search_path)
print_level_info(children_fields)
print "\n"
def walk_through_parsed_fields():
print_level_info(spec.parsed_fields())
def get_serialization_type_name(type_name):
if type_name in type_serialize_map:
return type_serialize_map[type_name]
else:
raise Exception("Type {0} not supported, add to type_serialize_map!".format(type_name))
def add_serialize_functions(fields, scope_name):
for field in fields:
if (not field.is_header):
if (field.is_builtin):
if (not field.is_array):
print "\tserialize"+str(get_serialization_type_name(field.type))+"(input->"+scope_name+str(field.name)+", microCDRWriter);"
else:
print "\tserialize"+str(get_serialization_type_name(field.base_type))+"Array(input->"+scope_name+str(field.name)+", "+str(field.array_len)+", microCDRWriter);"
else:
name = field.name
children_fields = get_children_fields(field.base_type, search_path)
if (scope_name): name = scope_name + name
if (not field.is_array):
add_serialize_functions(children_fields, name + '.')
else:
for i in xrange(field.array_len):
add_serialize_functions(children_fields, name + ('[%d].' %i))
def add_deserialize_functions(fields, scope_name):
for field in fields:
if (not field.is_header):
if (field.is_builtin):
if (not field.is_array):
print "\tdeserialize"+str(get_serialization_type_name(field.type))+"(&output->"+scope_name+str(field.name)+", microCDRReader);"
else:
for i in xrange(field.array_len):
print "\tdeserialize"+str(get_serialization_type_name(field.base_type))+"(&output->"+scope_name+str(field.name)+ str('[%d]' %i) +", microCDRReader);"
else:
name = field.name
children_fields = get_children_fields(field.base_type, search_path)
if (scope_name): name = scope_name + name
if (not field.is_array):
add_deserialize_functions(children_fields, name + '.')
else:
for i in xrange(field.array_len):
add_deserialize_functions(children_fields, name + ('[%d].' %i))
def add_code_to_serialize():
# sort fields (using a stable sort) as in the declaration of the type
sorted_fields = sorted(spec.parsed_fields(), key=sizeof_field_type, reverse=True)
add_serialize_functions(sorted_fields, "")
def add_code_to_deserialize():
# sort fields (using a stable sort) as in the declaration of the type
sorted_fields = sorted(spec.parsed_fields(), key=sizeof_field_type, reverse=True)
add_deserialize_functions(sorted_fields, "")
}@
void serialize_@(topic_name)(const struct @(uorb_struct) *input, char *output, uint32_t *length, struct microCDR *microCDRWriter)
{
if (nullptr == input || nullptr == output || nullptr == length || nullptr == microCDRWriter) return;
resetStaticMicroCDRForSerialize(microCDRWriter);
@add_code_to_serialize()
(*length) = microCDRWriter->m_microBuffer->m_serializedBuffer;
}
void deserialize_@(topic_name)(struct @(uorb_struct) *output, char *input, struct microCDR *microCDRReader)
{
if (nullptr == output || nullptr == input || nullptr == microCDRReader) return;
resetStaticMicroCDRForDeserialize(microCDRReader);
@add_code_to_deserialize()
}

7
msg/templates/uorb/msg.h.template
View File

@ -108,6 +108,8 @@ def print_parsed_fields():
print_field_def(field)
}@
struct microCDR;
#ifdef __cplusplus
@#class @(uorb_struct) {
struct __EXPORT @(uorb_struct) {
@ -118,6 +120,7 @@ struct @(uorb_struct) {
@# timestamp at the beginning of each topic is required for logger
uint64_t timestamp; // required for logger
@print_parsed_fields()
#ifdef __cplusplus
@# Constants again c++-ified
@{
@ -134,6 +137,10 @@ for constant in spec.constants:
#endif
};
void serialize_@(topic_name)(const struct @(uorb_struct) *input, char *output, uint32_t *length, struct microCDR *microCDRWriter);
void deserialize_@(topic_name)(struct @(uorb_struct) *output, char *input, struct microCDR *microCDRReader);
/* register this as object request broker structure */
@[for multi_topic in topics]@
ORB_DECLARE(@multi_topic);

154
msg/templates/urtps/Publisher.cxx.template Normal file
View File

@ -0,0 +1,154 @@
@###############################################
@#
@# EmPy template for generating <msg>_uRTPS_UART.cpp file
@#
@###############################################
@# Start of Template
@#
@# Context:
@# - msgs (List) list of all msg files
@# - multi_topics (List) list of all multi-topic names
@###############################################
@{
import genmsg.msgs
import gencpp
from px_generate_uorb_topic_helper import * # this is in Tools/
topic = spec.short_name
}@
/****************************************************************************
*
* Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima).
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* 3. Neither the name of the copyright holder nor the names of its contributors
* may be used to endorse or promote products derived from this software without
* specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/*!
* @@file @(topic)_Publisher.cpp
* This file contains the implementation of the publisher functions.
*
* This file was generated by the tool fastcdrgen.
*/
#include <fastrtps/participant/Participant.h>
#include <fastrtps/attributes/ParticipantAttributes.h>
#include <fastrtps/publisher/Publisher.h>
#include <fastrtps/attributes/PublisherAttributes.h>
#include <fastrtps/Domain.h>
#include <fastrtps/utils/eClock.h>
#include "@(topic)_Publisher.h"
@(topic)_Publisher::@(topic)_Publisher() : mp_participant(nullptr), mp_publisher(nullptr) {}
@(topic)_Publisher::~@(topic)_Publisher() { Domain::removeParticipant(mp_participant);}
bool @(topic)_Publisher::init()
{
// Create RTPSParticipant
ParticipantAttributes PParam;
PParam.rtps.builtin.domainId = 0;
PParam.rtps.builtin.leaseDuration = c_TimeInfinite;
PParam.rtps.setName("Participant_publisher"); //You can put here the name you want
mp_participant = Domain::createParticipant(PParam);
if(mp_participant == nullptr)
return false;
//Register the type
Domain::registerType(mp_participant,(TopicDataType*) &myType);
// Create Publisher
PublisherAttributes Wparam;
Wparam.topic.topicKind = NO_KEY;
Wparam.topic.topicDataType = myType.getName(); //This type MUST be registered
Wparam.topic.topicName = "@(topic)_PubSubTopic";
mp_publisher = Domain::createPublisher(mp_participant,Wparam,(PublisherListener*)&m_listener);
if(mp_publisher == nullptr)
return false;
//std::cout << "Publisher created, waiting for Subscribers." << std::endl;
return true;
}
void @(topic)_Publisher::PubListener::onPublicationMatched(Publisher* pub,MatchingInfo& info)
{
if (info.status == MATCHED_MATCHING)
{
n_matched++;
std::cout << "Publisher matched" << std::endl;
}
else
{
n_matched--;
std::cout << "Publisher unmatched" << std::endl;
}
}
void @(topic)_Publisher::run()
{
while(m_listener.n_matched == 0)
{
eClock::my_sleep(250); // Sleep 250 ms
}
// Publication code
@(topic)_ st;
/* Initialize your structure here */
int msgsent = 0;
char ch = 'y';
do
{
if(ch == 'y')
{
mp_publisher->write(&st); ++msgsent;
std::cout << "Sending sample, count=" << msgsent << ", send another sample?(y-yes,n-stop): ";
}
else if(ch == 'n')
{
std::cout << "Stopping execution " << std::endl;
break;
}
else
{
std::cout << "Command " << ch << " not recognized, please enter \"y/n\":";
}
}while(std::cin >> ch);
}
void @(topic)_Publisher::publish(@(topic)_* st)
{
mp_publisher->write(st);
}

92
msg/templates/urtps/Publisher.h.template Normal file
View File

@ -0,0 +1,92 @@
@###############################################
@#
@# EmPy template for generating <msg>_uRTPS_UART.cpp file
@#
@###############################################
@# Start of Template
@#
@# Context:
@# - msgs (List) list of all msg files
@# - multi_topics (List) list of all multi-topic names
@###############################################
@{
import genmsg.msgs
import gencpp
from px_generate_uorb_topic_helper import * # this is in Tools/
topic = spec.short_name
}@
/****************************************************************************
*
* Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima).
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* 3. Neither the name of the copyright holder nor the names of its contributors
* may be used to endorse or promote products derived from this software without
* specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/*!
* @@file @(topic)_Publisher.h
* This header file contains the declaration of the publisher functions.
*
* This file was generated by the tool fastcdrgen.
*/
#ifndef _@(topic)__PUBLISHER_H_
#define _@(topic)__PUBLISHER_H_
#include <fastrtps/fastrtps_fwd.h>
#include <fastrtps/publisher/PublisherListener.h>
#include "@(topic)_PubSubTypes.h"
using namespace eprosima::fastrtps;
class @(topic)_Publisher
{
public:
@(topic)_Publisher();
virtual ~@(topic)_Publisher();
bool init();
void run();
void publish(@(topic)_* st);
private:
Participant *mp_participant;
Publisher *mp_publisher;
class PubListener : public PublisherListener
{
public:
PubListener() : n_matched(0){};
~PubListener(){};
void onPublicationMatched(Publisher* pub,MatchingInfo& info);
int n_matched;
} m_listener;
@(topic)_PubSubType myType;
};
#endif // _@(topic)__PUBLISHER_H_

159
msg/templates/urtps/RtpsTopics.cxx.template Normal file
View File

@ -0,0 +1,159 @@
@###############################################
@#
@# EmPy template for generating RtpsTopics.cxx file
@#
@###############################################
@# Start of Template
@#
@# Context:
@# - msgs (List) list of all msg files
@###############################################
@{
import genmsg.msgs
import gencpp
from px_generate_uorb_topic_helper import * # this is in Tools/
from px_generate_uorb_topic_files import MsgScope # this is in Tools/
from message_id import * # this is in Tools/
topic_names = [single_spec.short_name for single_spec in spec]
send_topics = [s.short_name for idx, s in enumerate(spec) if scope[idx] == MsgScope.SEND]
recv_topics = [s.short_name for idx, s in enumerate(spec) if scope[idx] == MsgScope.RECEIVE]
}@
/****************************************************************************
*
* Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima).
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* 3. Neither the name of the copyright holder nor the names of its contributors
* may be used to endorse or promote products derived from this software without
* specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "RtpsTopics.h"
bool RtpsTopics::init()
{
@[if recv_topics]@
// Initialise subscribers
@[for topic in recv_topics]@
if (_@(topic)_sub.init()) {
std::cout << "@(topic) subscriber started" << std::endl;
} else {
std::cout << "ERROR starting @(topic) subscriber" << std::endl;
return false;
}
@[end for]@
@[end if]@
@[if send_topics]@
// Initialise publishers
@[for topic in send_topics]@
if (_@(topic)_pub.init()) {
std::cout << "@(topic) publisher started" << std::endl;
} else {
std::cout << "ERROR starting @(topic) publisher" << std::endl;
return false;
}
@[end for]@
@[end if]@
return true;
}
@[if send_topics]@
void RtpsTopics::publish(char topic_ID, char data_buffer[], size_t len)
{
switch (topic_ID)
{
@[for topic in send_topics]@
case @(message_id(topic)): // @(topic)
{
@(topic)_ st;
eprosima::fastcdr::FastBuffer cdrbuffer(data_buffer, len);
eprosima::fastcdr::Cdr cdr_des(cdrbuffer);
st.deserialize(cdr_des);
_@(topic)_pub.publish(&st);
}
break;
@[end for]@
default:
printf("Unexpected topic ID to publish\n");
break;
}
}
@[end if]@
@[if recv_topics]@
bool RtpsTopics::hasMsg(char *topic_ID)
{
if (nullptr == topic_ID) return false;
*topic_ID = 0;
while (_next_sub_idx < @(len(recv_topics)) && 0 == *topic_ID)
{
switch (_sub_topics[_next_sub_idx])
{
@[for topic in recv_topics]@
case @(message_id(topic)): if (_@(topic)_sub.hasMsg()) *topic_ID = @(message_id(topic)); break;
@[end for]@
default:
printf("Unexpected topic ID to check hasMsg\n");
break;
}
_next_sub_idx++;
}
if (0 == *topic_ID)
{
_next_sub_idx = 0;
return false;
}
return true;
}
bool RtpsTopics::getMsg(const char topic_ID, eprosima::fastcdr::Cdr &scdr)
{
bool ret = false;
switch (topic_ID)
{
@[for topic in recv_topics]@
case @(message_id(topic)): // @(topic)
if (_@(topic)_sub.hasMsg())
{
@(topic)_ msg = _@(topic)_sub.getMsg();
msg.serialize(scdr);
ret = true;
}
break;
@[end for]@
default:
printf("Unexpected topic ID '%hhu' to getMsg\n", topic_ID);
break;
}
return ret;
}
@[end if]@

95
msg/templates/urtps/RtpsTopics.h.template Normal file
View File

@ -0,0 +1,95 @@
@###############################################
@#
@# EmPy template for generating RtpsTopics.h file
@#
@###############################################
@# Start of Template
@#
@# Context:
@# - msgs (List) list of all msg files
@###############################################
@{
import genmsg.msgs
import gencpp
from px_generate_uorb_topic_helper import * # this is in Tools/
from px_generate_uorb_topic_files import MsgScope # this is in Tools/
from message_id import * # this is in Tools/
topic_names = [single_spec.short_name for single_spec in spec]
send_topics = [s.short_name for idx, s in enumerate(spec) if scope[idx] == MsgScope.SEND]
recv_topics = [s.short_name for idx, s in enumerate(spec) if scope[idx] == MsgScope.RECEIVE]
}@
/****************************************************************************
*
* Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima).
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* 3. Neither the name of the copyright holder nor the names of its contributors
* may be used to endorse or promote products derived from this software without
* specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include <fastcdr/Cdr.h>
@[for topic in send_topics]@
#include "@(topic)_Publisher.h"
@[end for]@
@[for topic in recv_topics]@
#include "@(topic)_Subscriber.h"
@[end for]@
class RtpsTopics {
public:
bool init();
@[if send_topics]@
void publish(char topic_ID, char data_buffer[], size_t len);
@[end if]@
@[if recv_topics]@
bool hasMsg(char *topic_ID);
bool getMsg(const char topic_ID, eprosima::fastcdr::Cdr &scdr);
@[end if]@
private:
@[if send_topics]@
// Publishers
@[for topic in send_topics]@
@(topic)_Publisher _@(topic)_pub;
@[end for]@
@[end if]@
@[if recv_topics]@
// Subscribers
@[for topic in recv_topics]@
@(topic)_Subscriber _@(topic)_sub;
@[end for]@
unsigned _next_sub_idx = 0;
char _sub_topics[@(len(recv_topics))] = {
@[for topic in recv_topics]@
@(message_id(topic)), // @(topic)
@[end for]@
};
@[end if]@
};

146
msg/templates/urtps/Subscriber.cxx.template Normal file
View File

@ -0,0 +1,146 @@
@###############################################
@#
@# EmPy template for generating <msg>_uRTPS_UART.cpp file
@#
@###############################################
@# Start of Template
@#
@# Context:
@# - msgs (List) list of all msg files
@# - multi_topics (List) list of all multi-topic names
@###############################################
@{
import genmsg.msgs
import gencpp
from px_generate_uorb_topic_helper import * # this is in Tools/
topic = spec.short_name
}@
/****************************************************************************
*
* Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima).
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* 3. Neither the name of the copyright holder nor the names of its contributors
* may be used to endorse or promote products derived from this software without
* specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/*!
* @@file @(topic)_Subscriber.cpp
* This file contains the implementation of the subscriber functions.
*
* This file was generated by the tool fastcdrgen.
*/
#include <fastrtps/participant/Participant.h>
#include <fastrtps/attributes/ParticipantAttributes.h>
#include <fastrtps/subscriber/Subscriber.h>
#include <fastrtps/attributes/SubscriberAttributes.h>
#include <fastrtps/Domain.h>
#include "@(topic)_Subscriber.h"
@(topic)_Subscriber::@(topic)_Subscriber() : mp_participant(nullptr), mp_subscriber(nullptr) {}
@(topic)_Subscriber::~@(topic)_Subscriber() { Domain::removeParticipant(mp_participant);}
bool @(topic)_Subscriber::init()
{
// Create RTPSParticipant
ParticipantAttributes PParam;
PParam.rtps.builtin.domainId = 0; //MUST BE THE SAME AS IN THE PUBLISHER
PParam.rtps.builtin.leaseDuration = c_TimeInfinite;
PParam.rtps.setName("Participant_subscriber"); //You can put the name you want
mp_participant = Domain::createParticipant(PParam);
if(mp_participant == nullptr)
return false;
//Register the type
Domain::registerType(mp_participant,(TopicDataType*) &myType);
// Create Subscriber
SubscriberAttributes Rparam;
Rparam.topic.topicKind = NO_KEY;
Rparam.topic.topicDataType = myType.getName(); //Must be registered before the creation of the subscriber
Rparam.topic.topicName = "@(topic)_PubSubTopic";
mp_subscriber = Domain::createSubscriber(mp_participant,Rparam,(SubscriberListener*)&m_listener);
if(mp_subscriber == nullptr)
return false;
return true;
}
void @(topic)_Subscriber::SubListener::onSubscriptionMatched(Subscriber* sub,MatchingInfo& info)
{
if (info.status == MATCHED_MATCHING)
{
n_matched++;
std::cout << "Subscriber matched" << std::endl;
}
else
{
n_matched--;
std::cout << "Subscriber unmatched" << std::endl;
}
}
void @(topic)_Subscriber::SubListener::onNewDataMessage(Subscriber* sub)
{
// Take data
if(sub->takeNextData(&msg, &m_info))
{
if(m_info.sampleKind == ALIVE)
{
// Print your structure data here.
++n_msg;
//std::cout << "Sample received, count=" << n_msg << std::endl;
has_msg = true;
}
}
}
void @(topic)_Subscriber::run()
{
std::cout << "Waiting for Data, press Enter to stop the Subscriber. "<<std::endl;
std::cin.ignore();
std::cout << "Shutting down the Subscriber." << std::endl;
}
bool @(topic)_Subscriber::hasMsg()
{
return m_listener.has_msg;
}
@(topic)_ @(topic)_Subscriber::getMsg()
{
m_listener.has_msg = false;
return m_listener.msg;
}

99
msg/templates/urtps/Subscriber.h.template Normal file
View File

@ -0,0 +1,99 @@
@###############################################
@#
@# EmPy template for generating <msg>_uRTPS_UART.cpp file
@#
@###############################################
@# Start of Template
@#
@# Context:
@# - msgs (List) list of all msg files
@# - multi_topics (List) list of all multi-topic names
@###############################################
@{
import genmsg.msgs
import gencpp
from px_generate_uorb_topic_helper import * # this is in Tools/
topic = spec.short_name
}@
/****************************************************************************
*
* Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima).
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* 3. Neither the name of the copyright holder nor the names of its contributors
* may be used to endorse or promote products derived from this software without
* specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/*!
* @@file @(topic)_Subscriber.h
* This header file contains the declaration of the subscriber functions.
*
* This file was generated by the tool fastcdrgen.
*/
#ifndef _@(topic)__SUBSCRIBER_H_
#define _@(topic)__SUBSCRIBER_H_
#include <fastrtps/fastrtps_fwd.h>
#include <fastrtps/subscriber/SubscriberListener.h>
#include <fastrtps/subscriber/SampleInfo.h>
#include "@(topic)_PubSubTypes.h"
using namespace eprosima::fastrtps;
class @(topic)_Subscriber
{
public:
@(topic)_Subscriber();
virtual ~@(topic)_Subscriber();
bool init();
void run();
bool hasMsg();
@(topic)_ getMsg();
private:
Participant *mp_participant;
Subscriber *mp_subscriber;
class SubListener : public SubscriberListener
{
public:
SubListener() : n_matched(0),n_msg(0){};
~SubListener(){};
void onSubscriptionMatched(Subscriber* sub,MatchingInfo& info);
void onNewDataMessage(Subscriber* sub);
SampleInfo_t m_info;
int n_matched;
int n_msg;
@(topic)_ msg;
bool has_msg = false;
} m_listener;
@(topic)_PubSubType myType;
};
#endif // _@(topic)__SUBSCRIBER_H_

280
msg/templates/urtps/microRTPS_agent.cxx.template Normal file
View File

@ -0,0 +1,280 @@
@###############################################
@#
@# EmPy template for generating microRTPS_agent.cpp file
@#
@###############################################
@# Start of Template
@#
@# Context:
@# - msgs (List) list of all msg files
@# - multi_topics (List) list of all multi-topic names
@###############################################
@{
import genmsg.msgs
import gencpp
from px_generate_uorb_topic_helper import * # this is in Tools/
from px_generate_uorb_topic_files import MsgScope # this is in Tools/
from message_id import * # this is in Tools/
topic_names = [single_spec.short_name for single_spec in spec]
send_topics = [s.short_name for idx, s in enumerate(spec) if scope[idx] == MsgScope.SEND]
recv_topics = [s.short_name for idx, s in enumerate(spec) if scope[idx] == MsgScope.RECEIVE]
}@
/****************************************************************************
*
* Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima).
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* 3. Neither the name of the copyright holder nor the names of its contributors
* may be used to endorse or promote products derived from this software without
* specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include <thread>
#include <unistd.h>
#include <poll.h>
#include <chrono>
#include <ctime>
#include <csignal>
#include <fastcdr/Cdr.h>
#include <fastcdr/FastCdr.h>
#include <fastcdr/exceptions/Exception.h>
#include <fastrtps/utils/eClock.h>
#include <fastrtps/Domain.h>
#include "microRTPS_transport.h"
#include "RtpsTopics.h"
#define BUFFER_SIZE 1024
// Default values
#define DEVICE "/dev/ttyACM0"
#define SLEEP_US 1
#define BAUDRATE 460800
#define POLL_MS 0
#define WAIT_CNST 2
#define DEFAULT_RECV_PORT 2020
#define DEFAULT_SEND_PORT 2019
using namespace eprosima;
using namespace eprosima::fastrtps;
volatile sig_atomic_t running = 1;
Transport_node *transport_node = nullptr;
RtpsTopics topics;
uint32_t total_sent = 0, sent = 0;
struct options {
enum class eTransports
{
UART,
UDP
};
eTransports transport = options::eTransports::UART;
char device[64] = DEVICE;
int sleep_us = SLEEP_US;
uint32_t baudrate = BAUDRATE;
int poll_ms = POLL_MS;
uint16_t recv_port = DEFAULT_RECV_PORT;
uint16_t send_port = DEFAULT_SEND_PORT;
} _options;
static void usage(const char *name)
{
printf("usage: %s [options]\n\n"
" -t <transport> [UART|UDP] Default UART\n"
" -d <device> UART device. Default /dev/ttyACM0\n"
" -w <sleep_time_us> Time in us for which each iteration sleep. Default 1ms\n"
" -b <baudrate> UART device baudrate. Default 460800\n"
" -p <poll_ms> Time in ms to poll over UART. Default 1ms\n"
" -r <reception port> UDP port for receiving. Default 2019\n"
" -s <sending port> UDP port for sending. Default 2020\n",
name);
}
static int parse_options(int argc, char **argv)
{
int ch;
while ((ch = getopt(argc, argv, "t:d:w:b:p:r:s:")) != EOF)
{
switch (ch)
{
case 't': _options.transport = strcmp(optarg, "UDP") == 0?
options::eTransports::UDP
:options::eTransports::UART; break;
case 'd': if (nullptr != optarg) strcpy(_options.device, optarg); break;
case 'w': _options.sleep_us = strtol(optarg, nullptr, 10); break;
case 'b': _options.baudrate = strtoul(optarg, nullptr, 10); break;
case 'p': _options.poll_ms = strtol(optarg, nullptr, 10); break;
case 'r': _options.recv_port = strtoul(optarg, nullptr, 10); break;
case 's': _options.send_port = strtoul(optarg, nullptr, 10); break;
default:
usage(argv[0]);
return -1;
}
}
if (optind < argc)
{
usage(argv[0]);
return -1;
}
return 0;
}
void signal_handler(int signum)
{
printf("Interrupt signal (%d) received.\n", signum);
running = 0;
transport_node->close();
}
@[if recv_topics]@
void t_send(void *data)
{
char data_buffer[BUFFER_SIZE] = {};
int length = 0;
char topic_ID = 255;
while (running)
{
// Send subscribed topics over UART
while (topics.hasMsg(&topic_ID))
{
eprosima::fastcdr::FastBuffer cdrbuffer(data_buffer, sizeof(data_buffer));
eprosima::fastcdr::Cdr scdr(cdrbuffer);
if (topics.getMsg(topic_ID, scdr))
{
length = scdr.getSerializedDataLength();
if (0 < (length = transport_node->write(topic_ID, scdr.getBufferPointer(), length)))
{
total_sent += length;
++sent;
}
}
}
usleep(_options.sleep_us);
}
}
@[end if]@
int main(int argc, char** argv)
{
if (-1 == parse_options(argc, argv))
{
printf("EXITING...\n");
return -1;
}
// register signal SIGINT and signal handler
signal(SIGINT, signal_handler);
switch (_options.transport)
{
case options::eTransports::UART:
{
transport_node = new UART_node(_options.device, _options.baudrate, _options.poll_ms);
printf("\nUART transport: device: %s; baudrate: %d; sleep: %dus; poll: %dms\n\n",
_options.device, _options.baudrate, _options.sleep_us, _options.poll_ms);
}
break;
case options::eTransports::UDP:
{
transport_node = new UDP_node(_options.recv_port, _options.send_port);
printf("\nUDP transport: recv port: %u; send port: %u; sleep: %dus\n\n",
_options.recv_port, _options.send_port, _options.sleep_us);
}
break;
default:
printf("EXITING...\n");
return -1;
}
if (0 > transport_node->init())
{
printf("EXITING...\n");
return -1;
}
sleep(1);
@[if send_topics]@
char data_buffer[BUFFER_SIZE] = {};
int received = 0, loop = 0;
int length = 0, total_read = 0;
bool receiving = false;
char topic_ID = 255;
std::chrono::time_point<std::chrono::steady_clock> start, end;
@[end if]@
topics.init();
running = true;
@[if recv_topics]@
std::thread sender_thread(t_send, nullptr);
@[end if]@
while (running)
{
@[if send_topics]@
++loop;
if (!receiving) start = std::chrono::steady_clock::now();
// Publish messages received from UART
while (0 < (length = transport_node->read(&topic_ID, data_buffer, BUFFER_SIZE)))
{
topics.publish(topic_ID, data_buffer, sizeof(data_buffer));
++received;
total_read += length;
receiving = true;
end = std::chrono::steady_clock::now();
}
if ((receiving && std::chrono::duration<double>(std::chrono::steady_clock::now() - end).count() > WAIT_CNST) ||
(!running && loop > 1))
{
std::chrono::duration<double> elapsed_secs = end - start;
printf("\nSENT: %lu messages - %lu bytes\n",
(unsigned long)sent, (unsigned long)total_sent);
printf("RECEIVED: %d messages - %d bytes; %d LOOPS - %.03f seconds - %.02fKB/s\n\n",
received, total_read, loop, elapsed_secs.count(), (double)total_read/(1000*elapsed_secs.count()));
received = sent = total_read = total_sent = 0;
receiving = false;
}
@[end if]@
usleep(_options.sleep_us);
}
@[if recv_topics]@
sender_thread.join();
@[end if]@
delete transport_node;
transport_node = nullptr;
return 0;
}

54
msg/templates/urtps/microRTPS_agent_CMakeLists.txt.template Normal file
View File

@ -0,0 +1,54 @@
################################################################################
#
# Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima).
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
#
# 1. Redistributions of source code must retain the above copyright notice, this
# list of conditions and the following disclaimer.
#
# 2. Redistributions in binary form must reproduce the above copyright notice,
# this list of conditions and the following disclaimer in the documentation
# and/or other materials provided with the distribution.
#
# 3. Neither the name of the copyright holder nor the names of its contributors
# may be used to endorse or promote products derived from this software without
# specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
################################################################################
cmake_minimum_required(VERSION 2.8.12)
project(micrortps_agent)
# Find requirements
find_package(fastrtps REQUIRED)
find_package(fastcdr REQUIRED)
# Set C++11
include(CheckCXXCompilerFlag)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_COMPILER_IS_CLANG OR
CMAKE_CXX_COMPILER_ID MATCHES "Clang")
check_cxx_compiler_flag(--std=c++11 SUPPORTS_CXX11)
if(SUPPORTS_CXX11)
add_compile_options(--std=c++11)
else()
message(FATAL_ERROR "Compiler doesn't support C++11")
endif()
endif()
file(GLOB MICRORTPS_AGENT_SOURCES *.cxx)
add_executable(micrortps_agent ${MICRORTPS_AGENT_SOURCES})
target_link_libraries(micrortps_agent fastrtps fastcdr)

465
msg/templates/urtps/microRTPS_transport.cxx Normal file
View File

@ -0,0 +1,465 @@
/****************************************************************************
*
* Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima).
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* 3. Neither the name of the copyright holder nor the names of its contributors
* may be used to endorse or promote products derived from this software without
* specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include <unistd.h>
#include <fcntl.h>
#include <termios.h>
#include <stdio.h>
#include <errno.h>
#include <sys/socket.h>
#include "microRTPS_transport.h"
#define DEFAULT_UART "/dev/ttyACM0"
/** CRC table for the CRC-16. The poly is 0x8005 (x^16 + x^15 + x^2 + 1) */
uint16_t const crc16_table[256] = {
0x0000, 0xC0C1, 0xC181, 0x0140, 0xC301, 0x03C0, 0x0280, 0xC241,
0xC601, 0x06C0, 0x0780, 0xC741, 0x0500, 0xC5C1, 0xC481, 0x0440,
0xCC01, 0x0CC0, 0x0D80, 0xCD41, 0x0F00, 0xCFC1, 0xCE81, 0x0E40,
0x0A00, 0xCAC1, 0xCB81, 0x0B40, 0xC901, 0x09C0, 0x0880, 0xC841,
0xD801, 0x18C0, 0x1980, 0xD941, 0x1B00, 0xDBC1, 0xDA81, 0x1A40,
0x1E00, 0xDEC1, 0xDF81, 0x1F40, 0xDD01, 0x1DC0, 0x1C80, 0xDC41,
0x1400, 0xD4C1, 0xD581, 0x1540, 0xD701, 0x17C0, 0x1680, 0xD641,
0xD201, 0x12C0, 0x1380, 0xD341, 0x1100, 0xD1C1, 0xD081, 0x1040,
0xF001, 0x30C0, 0x3180, 0xF141, 0x3300, 0xF3C1, 0xF281, 0x3240,
0x3600, 0xF6C1, 0xF781, 0x3740, 0xF501, 0x35C0, 0x3480, 0xF441,
0x3C00, 0xFCC1, 0xFD81, 0x3D40, 0xFF01, 0x3FC0, 0x3E80, 0xFE41,
0xFA01, 0x3AC0, 0x3B80, 0xFB41, 0x3900, 0xF9C1, 0xF881, 0x3840,
0x2800, 0xE8C1, 0xE981, 0x2940, 0xEB01, 0x2BC0, 0x2A80, 0xEA41,
0xEE01, 0x2EC0, 0x2F80, 0xEF41, 0x2D00, 0xEDC1, 0xEC81, 0x2C40,
0xE401, 0x24C0, 0x2580, 0xE541, 0x2700, 0xE7C1, 0xE681, 0x2640,
0x2200, 0xE2C1, 0xE381, 0x2340, 0xE101, 0x21C0, 0x2080, 0xE041,
0xA001, 0x60C0, 0x6180, 0xA141, 0x6300, 0xA3C1, 0xA281, 0x6240,
0x6600, 0xA6C1, 0xA781, 0x6740, 0xA501, 0x65C0, 0x6480, 0xA441,
0x6C00, 0xACC1, 0xAD81, 0x6D40, 0xAF01, 0x6FC0, 0x6E80, 0xAE41,
0xAA01, 0x6AC0, 0x6B80, 0xAB41, 0x6900, 0xA9C1, 0xA881, 0x6840,
0x7800, 0xB8C1, 0xB981, 0x7940, 0xBB01, 0x7BC0, 0x7A80, 0xBA41,
0xBE01, 0x7EC0, 0x7F80, 0xBF41, 0x7D00, 0xBDC1, 0xBC81, 0x7C40,
0xB401, 0x74C0, 0x7580, 0xB541, 0x7700, 0xB7C1, 0xB681, 0x7640,
0x7200, 0xB2C1, 0xB381, 0x7340, 0xB101, 0x71C0, 0x7080, 0xB041,
0x5000, 0x90C1, 0x9181, 0x5140, 0x9301, 0x53C0, 0x5280, 0x9241,
0x9601, 0x56C0, 0x5780, 0x9741, 0x5500, 0x95C1, 0x9481, 0x5440,
0x9C01, 0x5CC0, 0x5D80, 0x9D41, 0x5F00, 0x9FC1, 0x9E81, 0x5E40,
0x5A00, 0x9AC1, 0x9B81, 0x5B40, 0x9901, 0x59C0, 0x5880, 0x9841,
0x8801, 0x48C0, 0x4980, 0x8941, 0x4B00, 0x8BC1, 0x8A81, 0x4A40,
0x4E00, 0x8EC1, 0x8F81, 0x4F40, 0x8D01, 0x4DC0, 0x4C80, 0x8C41,
0x4400, 0x84C1, 0x8581, 0x4540, 0x8701, 0x47C0, 0x4680, 0x8641,
0x8201, 0x42C0, 0x4380, 0x8341, 0x4100, 0x81C1, 0x8081, 0x4040
};
Transport_node::Transport_node(): rx_buff_pos(0)
{
}
Transport_node::~Transport_node()
{
}
uint16_t Transport_node::crc16_byte(uint16_t crc, const uint8_t data)
{
return (crc >> 8) ^ crc16_table[(crc ^ data) & 0xff];
}
uint16_t Transport_node::crc16(uint8_t const *buffer, size_t len)
{
uint16_t crc = 0;
while (len--) crc = crc16_byte(crc, *buffer++);
return crc;
}
ssize_t Transport_node::read(char* topic_ID, char out_buffer[], size_t buffer_len)
{
if (nullptr == out_buffer || nullptr == topic_ID || !fds_OK()) return -1;
*topic_ID = 255;
ssize_t len = node_read((void*)(rx_buffer + rx_buff_pos), sizeof(rx_buffer) - rx_buff_pos);
if (len <= 0)
{
int errsv = errno;
if (errsv && EAGAIN != errsv && ETIMEDOUT != errsv)
{
printf("Read fail %d\n", errsv);
}
return len;
}
rx_buff_pos += len;
// We read some
size_t header_size = sizeof(struct Header);
if (rx_buff_pos < header_size) return 0; //but not enough
uint32_t msg_start_pos = 0;
for (msg_start_pos = 0; msg_start_pos <= rx_buff_pos - header_size; ++msg_start_pos)
{
if ('>' == rx_buffer[msg_start_pos] && memcmp(rx_buffer + msg_start_pos, ">>>", 3) == 0)
{
break;
}
}
// Start not found
if (msg_start_pos > rx_buff_pos - header_size)
{
printf(" (↓↓ %u)\n", msg_start_pos);
// All we've checked so far is garbage, drop it - but save unchecked bytes
memmove(rx_buffer, rx_buffer + msg_start_pos, rx_buff_pos - msg_start_pos);
rx_buff_pos = rx_buff_pos - msg_start_pos;
return -1;
}
/*
* [>,>,>,topic_ID,seq,payload_length_H,payload_length_L,CRCHigh,CRCLow,payloadStart, ... ,payloadEnd]
*/
struct Header *header = (struct Header *)&rx_buffer[msg_start_pos];
uint32_t payload_len = ((uint32_t)header->payload_len_h << 8) | header->payload_len_l;
// The message won't fit the buffer.
if (buffer_len < header_size + payload_len) return -EMSGSIZE;
// We do not have a complete message yet
if(msg_start_pos + header_size + payload_len > rx_buff_pos)
{
// If there's garbage at the beginning, drop it
if (msg_start_pos > 0)
{
printf(" (↓ %u)\n", msg_start_pos);
memmove(rx_buffer, rx_buffer + msg_start_pos, rx_buff_pos - msg_start_pos);
rx_buff_pos -= msg_start_pos;
}
return 0;
}
uint16_t read_crc = ((uint16_t)header->crc_h << 8) | header->crc_l;
uint16_t calc_crc = crc16((uint8_t*)rx_buffer + msg_start_pos + header_size, payload_len);
if (read_crc != calc_crc)
{
printf("BAD CRC %u != %u\n", read_crc, calc_crc);
printf(" (↓ %lu)\n", (unsigned long)(header_size + payload_len));
len = -1;
}
else
{
// copy message to outbuffer and set other return values
memmove(out_buffer, rx_buffer + msg_start_pos + header_size, payload_len);
*topic_ID = header->topic_ID;
len = payload_len + header_size;
}
// discard message from rx_buffer
rx_buff_pos -= header_size + payload_len;
memmove(rx_buffer, rx_buffer + msg_start_pos + header_size + payload_len, rx_buff_pos);
return len;
}
ssize_t Transport_node::write(const char topic_ID, char buffer[], size_t length)
{
if (!fds_OK()) return -1;
static struct Header header
{
.marker = {'>','>','>'},
.topic_ID = 0u,
.seq = 0u,
.payload_len_h = 0u,
.payload_len_l = 0u,
.crc_h = 0u,
.crc_l = 0u
};
static uint8_t seq = 0;
// [>,>,>,topic_ID,seq,payload_length,CRCHigh,CRCLow,payload_start, ... ,payload_end]
uint16_t crc = crc16((uint8_t*)buffer, length);
header.topic_ID = topic_ID;
header.seq = seq++;
header.payload_len_h = (length >> 8) & 0xff;
header.payload_len_l = length & 0xff;
header.crc_h = (crc >> 8) & 0xff;
header.crc_l = crc & 0xff;
ssize_t len = node_write(&header, sizeof(header));
if (len != sizeof(header)) goto err;
len = node_write(buffer, length);
if (len != ssize_t(length)) goto err;
return len + sizeof(header);
err:
//int errsv = errno;
//if (len == -1 ) printf(" => Writing error '%d'\n", errsv);
//else printf(" => Wrote '%ld' != length(%lu) error '%d'\n", (long)len, (unsigned long)length, errsv);
return len;
}
UART_node::UART_node(const char *_uart_name, uint32_t _baudrate, uint32_t _poll_ms):
uart_fd(-1),
baudrate(_baudrate),
poll_ms(_poll_ms)
{
if (nullptr != _uart_name) strcpy(uart_name, _uart_name);
}
UART_node::~UART_node()
{
close();
}
int UART_node::init()
{
// Open a serial port
uart_fd = open(uart_name, O_RDWR | O_NOCTTY | O_NONBLOCK);
if (uart_fd < 0)
{
printf("failed to open device: %s (%d)\n", uart_name, errno);
return -errno;
}
// If using shared UART, no need to set it up
if (baudrate == 0) {
return uart_fd;
}
// Try to set baud rate
struct termios uart_config;
int termios_state;
// Back up the original uart configuration to restore it after exit
if ((termios_state = tcgetattr(uart_fd, &uart_config)) < 0)
{
int errno_bkp = errno;
printf("ERR GET CONF %s: %d (%d)\n", uart_name, termios_state, errno);
close();
return -errno_bkp;
}
// Clear ONLCR flag (which appends a CR for every LF)
uart_config.c_oflag &= ~ONLCR;
// USB serial is indicated by /dev/ttyACM0
if (strcmp(uart_name, "/dev/ttyACM0") != 0 && strcmp(uart_name, "/dev/ttyACM1") != 0)
{
// Set baud rate
if (cfsetispeed(&uart_config, baudrate) < 0 || cfsetospeed(&uart_config, baudrate) < 0)
{
int errno_bkp = errno;
printf("ERR SET BAUD %s: %d (%d)\n", uart_name, termios_state, errno);
close();
return -errno_bkp;
}
}
if ((termios_state = tcsetattr(uart_fd, TCSANOW, &uart_config)) < 0)
{
int errno_bkp = errno;
printf("ERR SET CONF %s (%d)\n", uart_name, errno);
close();
return -errno_bkp;
}
char aux[64];
bool flush = false;
while (0 < ::read(uart_fd, (void*)&aux, 64))
{
//printf("%s ", aux);
flush = true;
usleep(1000);
}
if (flush) printf("flush\n");
else printf("no flush\n");
poll_fd[0].fd = uart_fd;
poll_fd[0].events = POLLIN;
return uart_fd;
}
bool UART_node::fds_OK()
{
return (-1 != uart_fd);
}
uint8_t UART_node::close()
{
if (-1 != uart_fd)
{
printf("Close UART\n");
::close(uart_fd);
uart_fd = -1;
memset(&poll_fd, 0, sizeof(poll_fd));
}
return 0;
}
ssize_t UART_node::node_read(void *buffer, size_t len)
{
if (nullptr == buffer || !fds_OK()) return -1;
ssize_t ret = 0;
int r = poll(poll_fd, 1, poll_ms);
if (r == 1 && (poll_fd[0].revents & POLLIN))
{
ret = ::read(uart_fd, buffer, len);
}
return ret;
}
ssize_t UART_node::node_write(void *buffer, size_t len)
{
if (nullptr == buffer || !fds_OK()) return -1;
return ::write(uart_fd, buffer, len);
}
UDP_node::UDP_node(uint16_t _udp_port_recv, uint16_t _udp_port_send):
sender_fd(-1),
receiver_fd(-1),
udp_port_recv(_udp_port_recv),
udp_port_send(_udp_port_send)
{
}
UDP_node::~UDP_node()
{
close();
}
int UDP_node::init()
{
if (0 > init_receiver(udp_port_recv) || 0 > init_sender(udp_port_send))
return -1;
return 0;
}
bool UDP_node::fds_OK()
{
return (-1 != sender_fd && -1 != receiver_fd);
}
int UDP_node::init_receiver(uint16_t udp_port)
{
#ifndef __PX4_NUTTX
// udp socket data
memset((char *)&receiver_inaddr, 0, sizeof(receiver_inaddr));
receiver_inaddr.sin_family = AF_INET;
receiver_inaddr.sin_port = htons(udp_port);
receiver_inaddr.sin_addr.s_addr = htonl(INADDR_ANY);
if ((receiver_fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0)
{
printf("create socket failed\n");
return -1;
}
printf("Trying to connect...\n");
if (bind(receiver_fd, (struct sockaddr *)&receiver_inaddr, sizeof(receiver_inaddr)) < 0)
{
printf("bind failed\n");
return -1;
}
printf("connected to server!\n");
#endif /* __PX4_NUTTX */
return 0;
}
int UDP_node::init_sender(uint16_t udp_port)
{
#ifndef __PX4_NUTTX
if ((sender_fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0)
{
printf("create socket failed\n");
return -1;
}
memset((char *) &sender_outaddr, 0, sizeof(sender_outaddr));
sender_outaddr.sin_family = AF_INET;
sender_outaddr.sin_port = htons(udp_port);
if (inet_aton("127.0.0.1" , &sender_outaddr.sin_addr) == 0)
{
printf("inet_aton() failed\n");
return -1;
}
#endif /* __PX4_NUTTX */
return 0;
}
uint8_t UDP_node::close()
{
#ifndef __PX4_NUTTX
if (sender_fd != -1)
{
printf("Close sender socket\n");
shutdown(sender_fd, SHUT_RDWR);
::close(sender_fd);
sender_fd = -1;
}
if (receiver_fd != -1)
{
printf("Close receiver socket\n");
shutdown(receiver_fd, SHUT_RDWR);
::close(receiver_fd);
receiver_fd = -1;
}
#endif /* __PX4_NUTTX */
return 0;
}
ssize_t UDP_node::node_read(void *buffer, size_t len)
{
if (nullptr == buffer || !fds_OK()) return -1;
int ret = 0;
#ifndef __PX4_NUTTX
// Blocking call
static socklen_t addrlen = sizeof(receiver_outaddr);
ret = recvfrom(receiver_fd, buffer, len, 0, (struct sockaddr*) &receiver_outaddr, &addrlen);
#endif /* __PX4_NUTTX */
return ret;
}
ssize_t UDP_node::node_write(void *buffer, size_t len)
{
if (nullptr == buffer || !fds_OK()) return -1;
int ret = 0;
#ifndef __PX4_NUTTX
ret = sendto(sender_fd, buffer, len, 0, (struct sockaddr *)&sender_outaddr, sizeof(sender_outaddr));
#endif /* __PX4_NUTTX */
return ret;
}

119
msg/templates/urtps/microRTPS_transport.h Normal file
View File

@ -0,0 +1,119 @@
/****************************************************************************
*
* Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima).
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* 1. Redistributions of source code must retain the above copyright notice, this
* list of conditions and the following disclaimer.
*
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
*
* 3. Neither the name of the copyright holder nor the names of its contributors
* may be used to endorse or promote products derived from this software without
* specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#include <string>
#include <cstring>
#include <arpa/inet.h>
#include <poll.h>
class Transport_node
{
public:
Transport_node();
virtual ~Transport_node();
virtual int init() {return 0;}
virtual uint8_t close() {return 0;}
ssize_t read(char* topic_ID, char out_buffer[], size_t buffer_len);
ssize_t write(const char topic_ID, char buffer[], size_t length);
protected:
virtual ssize_t node_read(void *buffer, size_t len) = 0;
virtual ssize_t node_write(void *buffer, size_t len) = 0;
virtual bool fds_OK() = 0;
uint16_t crc16_byte(uint16_t crc, const uint8_t data);
uint16_t crc16(uint8_t const *buffer, size_t len);
protected:
uint32_t rx_buff_pos;
char rx_buffer[1024] = {};
private:
struct __attribute__((packed)) Header
{
char marker[3];
uint8_t topic_ID;
uint8_t seq;
uint8_t payload_len_h;
uint8_t payload_len_l;
uint8_t crc_h;
uint8_t crc_l;
};
};
class UART_node: public Transport_node
{
public:
UART_node(const char *uart_name, uint32_t baudrate, uint32_t poll_ms);
virtual ~UART_node();
int init();
uint8_t close();
protected:
ssize_t node_read(void *buffer, size_t len);
ssize_t node_write(void *buffer, size_t len);
bool fds_OK();
int uart_fd;
char uart_name[64] = {};
uint32_t baudrate;
uint32_t poll_ms;
struct pollfd poll_fd[1] = {};
};
class UDP_node: public Transport_node
{
public:
UDP_node(uint16_t udp_port_recv, uint16_t udp_port_send);
virtual ~UDP_node();
int init();
uint8_t close();
protected:
int init_receiver(uint16_t udp_port);
int init_sender(uint16_t udp_port);
ssize_t node_read(void *buffer, size_t len);
ssize_t node_write(void *buffer, size_t len);
bool fds_OK();
int sender_fd;
int receiver_fd;
uint16_t udp_port_recv;
uint16_t udp_port_send;
struct sockaddr_in sender_outaddr;
struct sockaddr_in receiver_inaddr;
struct sockaddr_in receiver_outaddr;
};

80
msg/templates/urtps/msg.idl.template Normal file
View File

@ -0,0 +1,80 @@
@###############################################
@#
@# ROS message to IDL converter
@#
@# EmPy template for generating <msg>.idl files
@#
@################################################################################
@#
@# Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima).
@#
@# Redistribution and use in source and binary forms, with or without
@# modification, are permitted provided that the following conditions are met:
@#
@# 1. Redistributions of source code must retain the above copyright notice, this
@# list of conditions and the following disclaimer.
@#
@# 2. Redistributions in binary form must reproduce the above copyright notice,
@# this list of conditions and the following disclaimer in the documentation
@# and/or other materials provided with the distribution.
@#
@# 3. Neither the name of the copyright holder nor the names of its contributors
@# may be used to endorse or promote products derived from this software without
@# specific prior written permission.
@#
@# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
@# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
@# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
@# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
@# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
@# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
@# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
@# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
@# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
@# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
@# POSSIBILITY OF SUCH DAMAGE.
@#
@################################################################################
@{
import genmsg.msgs
import gencpp
from px_generate_uorb_topic_helper import * # this is in Tools/
uorb_struct = '%s_s'%spec.short_name
topic_name = spec.short_name
sorted_fields = sorted(spec.parsed_fields(), key=sizeof_field_type, reverse=True)
struct_size, padding_end_size = add_padding_bytes(sorted_fields, search_path)
}@
@#################################################
@# Searching for serialize function per each field
@#################################################
@{
def get_idl_type_name(field_type):
if field_type in type_idl_map:
return type_idl_map[field_type]
else:
(package, name) = genmsg.names.package_resource_name(field_type)
return name
def add_msg_field(field):
if (not field.is_header):
if (not field.is_array):
print '\t' + str(get_idl_type_name(field.type)) + ' ' + field.name + ';'
else:
print '\t' + str(get_idl_type_name(field.base_type)) + ' ' + field.name + '[' +str(field.array_len)+ '];'
def add_msg_fields():
# sort fields (using a stable sort) as in the declaration of the type
sorted_fields = sorted(spec.parsed_fields(), key=sizeof_field_type, reverse=True)
for field in sorted_fields:
add_msg_field(field)
}@
@
struct @(spec.short_name)_
{
@add_msg_fields()
}; // struct @(spec.short_name)_

42
src/drivers/protocol_splitter/CMakeLists.txt Normal file
View File

@ -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 :

603
src/drivers/protocol_splitter/protocol_splitter.cpp Normal file
View File

@ -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;
}

1
src/lib/micro-CDR Submodule

@ -0,0 +1 @@
Subproject commit f2193957eb36365680647c6ed07a8bf8a3e71a9e

4
src/modules/mavlink/CMakeLists.txt
View File

@ -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

385
src/modules/micrortps_bridge/README.md Normal file
View File

@ -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.
![basic example flow](res/basic_example_flow.png)

115
src/modules/micrortps_bridge/hello_world.md Normal file
View File

@ -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, &microCDRReader);
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
...
```

86
src/modules/micrortps_bridge/micrortps_client/CMakeLists.txt Normal file
View File

@ -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 :

BIN
src/modules/micrortps_bridge/res/basic_example_flow.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 178 KiB

87
src/modules/micrortps_bridge/throughput_test.md Normal file
View File

@ -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, &microCDRWriter);
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
```