forked from Archive/PX4-Autopilot
rtps: add uorb_rtps_classifier.py iot classify RTPS topics as "send", "receive" or "ignore"
This commit is contained in:
parent
92a1594430
commit
a9771f13d1
|
@ -44,6 +44,7 @@ import argparse
|
|||
import shutil
|
||||
import px_generate_uorb_topic_files
|
||||
import px_generate_uorb_topic_helper
|
||||
import uorb_rtps_classifier
|
||||
import subprocess
|
||||
import glob
|
||||
import errno
|
||||
|
@ -54,48 +55,17 @@ except ImportError:
|
|||
"Failed to import yaml. You may need to install it with 'sudo pip install pyyaml'")
|
||||
|
||||
|
||||
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
|
||||
|
||||
|
||||
def parse_yaml_msg_id_file(yaml_file):
|
||||
def check_rtps_id_uniqueness(classifier):
|
||||
"""
|
||||
Parses a yaml file into a dict
|
||||
Checks if:
|
||||
1. there are no ID's for different msgs repeated on the map
|
||||
2. the same msg is set to be sent/received and unclassified at the same time
|
||||
"""
|
||||
try:
|
||||
with open(yaml_file, 'r') as f:
|
||||
return yaml.load(f)
|
||||
except OSError as e:
|
||||
if e.errno == errno.ENOENT:
|
||||
raise IOError(errno.ENOENT, os.strerror(errno.ENOENT), yaml_file)
|
||||
else:
|
||||
raise
|
||||
msgs_to_use = classifier.msgs_to_send
|
||||
msgs_to_use.update(classifier.msgs_to_receive)
|
||||
msgs_to_ignore = classifier.msgs_to_ignore
|
||||
|
||||
|
||||
def get_used_rtps_ids(msg_id_map):
|
||||
msg_ids = {}
|
||||
for dict in msg_id_map['rtps']:
|
||||
msg_ids.update({dict['msg']: dict['id']})
|
||||
return msg_ids
|
||||
|
||||
|
||||
def check_rtps_id_uniqueness(msg_id_map):
|
||||
"""
|
||||
Checks if there are no ID's repeated on the map
|
||||
"""
|
||||
msg_ids = get_used_rtps_ids(msg_id_map)
|
||||
|
||||
used_ids = msg_ids.values()
|
||||
used_ids = msgs_to_use.values()
|
||||
used_ids.sort()
|
||||
|
||||
repeated_keys = dict()
|
||||
|
@ -111,9 +81,9 @@ def check_rtps_id_uniqueness(msg_id_map):
|
|||
", ".join('%d' % id for id in px_generate_uorb_topic_helper.check_available_ids(used_ids)))
|
||||
|
||||
|
||||
default_client_out = get_absolute_path(
|
||||
default_client_out = px_generate_uorb_topic_helper.get_absolute_path(
|
||||
"src/modules/micrortps_bridge/micrortps_client")
|
||||
default_agent_out = get_absolute_path(
|
||||
default_agent_out = px_generate_uorb_topic_helper.get_absolute_path(
|
||||
"src/modules/micrortps_bridge/micrortps_agent")
|
||||
default_uorb_templates_dir = "templates/uorb_microcdr"
|
||||
default_urtps_templates_dir = "templates/urtps"
|
||||
|
@ -122,10 +92,6 @@ default_package_name = px_generate_uorb_topic_files.PACKAGE
|
|||
|
||||
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",
|
||||
|
@ -141,11 +107,11 @@ parser.add_argument("-l", "--generate-cmakelists", dest='cmakelists',
|
|||
parser.add_argument("-t", "--topic-msg-dir", dest='msgdir', type=str,
|
||||
help="Topics message dir, by default msg/", default="msg")
|
||||
parser.add_argument("-b", "--uorb-templates-dir", dest='uorb_templates', type=str,
|
||||
help="uORB templates dir, by default msg_dir/templates/uorb_microcdr", default=default_uorb_templates_dir)
|
||||
help="uORB templates dir, by default msgdir/templates/uorb_microcdr", default=default_uorb_templates_dir)
|
||||
parser.add_argument("-q", "--urtps-templates-dir", dest='urtps_templates', type=str,
|
||||
help="uRTPS templates dir, by default msg_dir/templates/urtps", default=default_urtps_templates_dir)
|
||||
help="uRTPS templates dir, by default msgdir/templates/urtps", default=default_urtps_templates_dir)
|
||||
parser.add_argument("-y", "--rtps-ids-file", dest='yaml_file', type=str,
|
||||
help="RTPS msg IDs definition file, relative to the msg_dir, by default tools/uorb_rtps_message_ids.yaml", default=default_rtps_id_file)
|
||||
help="RTPS msg IDs definition file, relative to the msgdir, by default tools/uorb_rtps_message_ids.yaml", default=default_rtps_id_file)
|
||||
parser.add_argument("-p", "--package", dest='package', type=str,
|
||||
help="Msg package naming, by default px4", default=default_package_name)
|
||||
parser.add_argument("-o", "--agent-outdir", dest='agentdir', type=str, nargs=1,
|
||||
|
@ -166,13 +132,7 @@ if len(sys.argv) <= 1:
|
|||
|
||||
# Parse arguments
|
||||
args = parser.parse_args()
|
||||
msg_folder = get_absolute_path(args.msgdir)
|
||||
msg_files_send = []
|
||||
msg_files_receive = []
|
||||
if args.send:
|
||||
msg_files_send = [get_absolute_path(msg) for msg in args.send]
|
||||
if args.receive:
|
||||
msg_files_receive = [get_absolute_path(msg) for msg in args.receive]
|
||||
msg_folder = px_generate_uorb_topic_helper.get_absolute_path(args.msgdir)
|
||||
package = args.package
|
||||
agent = args.agent
|
||||
client = args.client
|
||||
|
@ -181,12 +141,13 @@ cmakelists = args.cmakelists
|
|||
del_tree = args.del_tree
|
||||
px_generate_uorb_topic_files.append_to_include_path(
|
||||
{msg_folder}, px_generate_uorb_topic_files.INCL_DEFAULT, package)
|
||||
agent_out_dir = get_absolute_path(args.agentdir)
|
||||
client_out_dir = get_absolute_path(args.clientdir)
|
||||
agent_out_dir = px_generate_uorb_topic_helper.get_absolute_path(args.agentdir)
|
||||
client_out_dir = px_generate_uorb_topic_helper.get_absolute_path(
|
||||
args.clientdir)
|
||||
gen_idl = args.gen_idl
|
||||
idl_dir = args.idl_dir
|
||||
if idl_dir != '':
|
||||
idl_dir = get_absolute_path(args.idl_dir)
|
||||
idl_dir = px_generate_uorb_topic_helper.get_absolute_path(args.idl_dir)
|
||||
else:
|
||||
idl_dir = os.path.join(agent_out_dir, "idl")
|
||||
|
||||
|
@ -196,11 +157,12 @@ if args.fastrtpsgen is None or args.fastrtpsgen == "":
|
|||
else:
|
||||
# Path to fastrtpsgen is explicitly specified
|
||||
fastrtpsgen_path = os.path.join(
|
||||
get_absolute_path(args.fastrtpsgen), "/fastrtpsgen")
|
||||
px_generate_uorb_topic_helper.get_absolute_path(args.fastrtpsgen), "/fastrtpsgen")
|
||||
fastrtpsgen_include = args.fastrtpsgen_include
|
||||
if fastrtpsgen_include is not None and fastrtpsgen_include != '':
|
||||
fastrtpsgen_include = "-I " + \
|
||||
get_absolute_path(args.fastrtpsgen_include) + " "
|
||||
px_generate_uorb_topic_helper.get_absolute_path(
|
||||
args.fastrtpsgen_include) + " "
|
||||
|
||||
# If nothing specified it's generated both
|
||||
if agent == False and client == False:
|
||||
|
@ -234,70 +196,70 @@ if agent and os.path.isdir(os.path.join(agent_out_dir, "idl")):
|
|||
uorb_templates_dir = os.path.join(msg_folder, args.uorb_templates)
|
||||
urtps_templates_dir = os.path.join(msg_folder, args.urtps_templates)
|
||||
# parse yaml file into a map of ids
|
||||
rtps_ids = parse_yaml_msg_id_file(os.path.join(msg_folder, args.yaml_file))
|
||||
classifier = Classifier(px_generate_uorb_topic_helper.parse_yaml_msg_id_file(os.path.join(msg_folder, args.yaml_file), msg_folder)
|
||||
# check if there are no ID's repeated
|
||||
check_rtps_id_uniqueness(rtps_ids)
|
||||
check_rtps_id_uniqueness(classifier)
|
||||
|
||||
|
||||
uRTPS_CLIENT_TEMPL_FILE = 'microRTPS_client.cpp.template'
|
||||
uRTPS_AGENT_TOPICS_H_TEMPL_FILE = 'RtpsTopics.h.template'
|
||||
uRTPS_AGENT_TOPICS_SRC_TEMPL_FILE = 'RtpsTopics.cpp.template'
|
||||
uRTPS_AGENT_TEMPL_FILE = 'microRTPS_agent.cpp.template'
|
||||
uRTPS_AGENT_CMAKELISTS_TEMPL_FILE = 'microRTPS_agent_CMakeLists.txt.template'
|
||||
uRTPS_PUBLISHER_SRC_TEMPL_FILE = 'Publisher.cpp.template'
|
||||
uRTPS_PUBLISHER_H_TEMPL_FILE = 'Publisher.h.template'
|
||||
uRTPS_SUBSCRIBER_SRC_TEMPL_FILE = 'Subscriber.cpp.template'
|
||||
uRTPS_SUBSCRIBER_H_TEMPL_FILE = 'Subscriber.h.template'
|
||||
uRTPS_CLIENT_TEMPL_FILE='microRTPS_client.cpp.template'
|
||||
uRTPS_AGENT_TOPICS_H_TEMPL_FILE='RtpsTopics.h.template'
|
||||
uRTPS_AGENT_TOPICS_SRC_TEMPL_FILE='RtpsTopics.cpp.template'
|
||||
uRTPS_AGENT_TEMPL_FILE='microRTPS_agent.cpp.template'
|
||||
uRTPS_AGENT_CMAKELISTS_TEMPL_FILE='microRTPS_agent_CMakeLists.txt.template'
|
||||
uRTPS_PUBLISHER_SRC_TEMPL_FILE='Publisher.cpp.template'
|
||||
uRTPS_PUBLISHER_H_TEMPL_FILE='Publisher.h.template'
|
||||
uRTPS_SUBSCRIBER_SRC_TEMPL_FILE='Subscriber.cpp.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:
|
||||
if classifier.msg_files_send:
|
||||
for msg_file in classifier.msg_files_send:
|
||||
if gen_idl:
|
||||
if out_dir != agent_out_dir:
|
||||
px_generate_uorb_topic_files.generate_idl_file(msg_file, os.path.join(out_dir, "/idl"), urtps_templates_dir,
|
||||
package, px_generate_uorb_topic_files.INCL_DEFAULT, rtps_ids)
|
||||
package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_id_map)
|
||||
else:
|
||||
px_generate_uorb_topic_files.generate_idl_file(msg_file, idl_dir, urtps_templates_dir,
|
||||
package, px_generate_uorb_topic_files.INCL_DEFAULT, rtps_ids)
|
||||
package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_id_map)
|
||||
px_generate_uorb_topic_files.generate_topic_file(msg_file, out_dir, urtps_templates_dir,
|
||||
package, px_generate_uorb_topic_files.INCL_DEFAULT, rtps_ids, uRTPS_PUBLISHER_SRC_TEMPL_FILE)
|
||||
package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_id_map, uRTPS_PUBLISHER_SRC_TEMPL_FILE)
|
||||
px_generate_uorb_topic_files.generate_topic_file(msg_file, out_dir, urtps_templates_dir,
|
||||
package, px_generate_uorb_topic_files.INCL_DEFAULT, rtps_ids, uRTPS_PUBLISHER_H_TEMPL_FILE)
|
||||
package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_id_map, uRTPS_PUBLISHER_H_TEMPL_FILE)
|
||||
|
||||
if msg_files_receive:
|
||||
for msg_file in msg_files_receive:
|
||||
if classifier.msg_files_receive:
|
||||
for msg_file in classifier.msg_files_receive:
|
||||
if gen_idl:
|
||||
if out_dir != agent_out_dir:
|
||||
px_generate_uorb_topic_files.generate_idl_file(msg_file, os.path.join(out_dir, "/idl"), urtps_templates_dir,
|
||||
package, px_generate_uorb_topic_files.INCL_DEFAULT, rtps_ids)
|
||||
package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_id_map)
|
||||
else:
|
||||
px_generate_uorb_topic_files.generate_idl_file(msg_file, idl_dir, urtps_templates_dir,
|
||||
package, px_generate_uorb_topic_files.INCL_DEFAULT, rtps_ids)
|
||||
package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_id_map)
|
||||
px_generate_uorb_topic_files.generate_topic_file(msg_file, out_dir, urtps_templates_dir,
|
||||
package, px_generate_uorb_topic_files.INCL_DEFAULT, rtps_ids, uRTPS_SUBSCRIBER_SRC_TEMPL_FILE)
|
||||
package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_id_map, uRTPS_SUBSCRIBER_SRC_TEMPL_FILE)
|
||||
px_generate_uorb_topic_files.generate_topic_file(msg_file, out_dir, urtps_templates_dir,
|
||||
package, px_generate_uorb_topic_files.INCL_DEFAULT, rtps_ids, uRTPS_SUBSCRIBER_H_TEMPL_FILE)
|
||||
package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_id_map, uRTPS_SUBSCRIBER_H_TEMPL_FILE)
|
||||
|
||||
px_generate_uorb_topic_files.generate_uRTPS_general(msg_files_send, msg_files_receive, out_dir, urtps_templates_dir,
|
||||
package, px_generate_uorb_topic_files.INCL_DEFAULT, rtps_ids, uRTPS_AGENT_TEMPL_FILE)
|
||||
px_generate_uorb_topic_files.generate_uRTPS_general(msg_files_send, msg_files_receive, out_dir, urtps_templates_dir,
|
||||
package, px_generate_uorb_topic_files.INCL_DEFAULT, rtps_ids, 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,
|
||||
package, px_generate_uorb_topic_files.INCL_DEFAULT, rtps_ids, uRTPS_AGENT_TOPICS_SRC_TEMPL_FILE)
|
||||
px_generate_uorb_topic_files.generate_uRTPS_general(classifier.msg_files_send, classifier.msg_files_receive, out_dir, urtps_templates_dir,
|
||||
package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_id_map, uRTPS_AGENT_TEMPL_FILE)
|
||||
px_generate_uorb_topic_files.generate_uRTPS_general(classifier.msg_files_send, classifier.msg_files_receive, out_dir, urtps_templates_dir,
|
||||
package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_id_map, uRTPS_AGENT_TOPICS_H_TEMPL_FILE)
|
||||
px_generate_uorb_topic_files.generate_uRTPS_general(classifier.msg_files_send, classifier.msg_files_receive, out_dir, urtps_templates_dir,
|
||||
package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_id_map, uRTPS_AGENT_TOPICS_SRC_TEMPL_FILE)
|
||||
if cmakelists:
|
||||
px_generate_uorb_topic_files.generate_uRTPS_general(msg_files_send, msg_files_receive, out_dir, urtps_templates_dir,
|
||||
package, px_generate_uorb_topic_files.INCL_DEFAULT, rtps_ids, uRTPS_AGENT_CMAKELISTS_TEMPL_FILE)
|
||||
px_generate_uorb_topic_files.generate_uRTPS_general(classifier.msg_files_send, classifier.msg_files_receive, out_dir, urtps_templates_dir,
|
||||
package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_id_map, uRTPS_AGENT_CMAKELISTS_TEMPL_FILE)
|
||||
|
||||
# Final steps to install agent
|
||||
mkdir_p(os.path.join(out_dir, "fastrtpsgen"))
|
||||
prev_cwd_path = os.getcwd()
|
||||
prev_cwd_path=os.getcwd()
|
||||
os.chdir(os.path.join(out_dir, "fastrtpsgen"))
|
||||
if not glob.glob(os.path.join(idl_dir, "*.idl")):
|
||||
raise Exception("No IDL files found in %s" % idl_dir)
|
||||
for idl_file in glob.glob(os.path.join(idl_dir, "*.idl")):
|
||||
ret = subprocess.call(fastrtpsgen_path + " -d " + out_dir +
|
||||
ret=subprocess.call(fastrtpsgen_path + " -d " + out_dir +
|
||||
"/fastrtpsgen -example x64Linux2.6gcc " + fastrtpsgen_include + idl_file, shell=True)
|
||||
if ret:
|
||||
raise Exception(
|
||||
|
@ -346,18 +308,18 @@ def generate_client(out_dir):
|
|||
|
||||
# Rename work in the default path
|
||||
if default_client_out != out_dir:
|
||||
def_file = os.path.join(default_client_out, "microRTPS_client.cpp")
|
||||
def_file=os.path.join(default_client_out, "microRTPS_client.cpp")
|
||||
if os.path.isfile(def_file):
|
||||
os.rename(def_file, def_file.replace(".cpp", ".cpp_"))
|
||||
def_file = os.path.join(default_client_out, "microRTPS_transport.cpp")
|
||||
def_file=os.path.join(default_client_out, "microRTPS_transport.cpp")
|
||||
if os.path.isfile(def_file):
|
||||
os.rename(def_file, def_file.replace(".cpp", ".cpp_"))
|
||||
def_file = os.path.join(default_client_out, "microRTPS_transport.h")
|
||||
def_file=os.path.join(default_client_out, "microRTPS_transport.h")
|
||||
if os.path.isfile(def_file):
|
||||
os.rename(def_file, def_file.replace(".h", ".h_"))
|
||||
|
||||
px_generate_uorb_topic_files.generate_uRTPS_general(msg_files_send, msg_files_receive, out_dir, uorb_templates_dir,
|
||||
package, px_generate_uorb_topic_files.INCL_DEFAULT, rtps_ids, uRTPS_CLIENT_TEMPL_FILE)
|
||||
px_generate_uorb_topic_files.generate_uRTPS_general(classifier.msg_files_send, classifier.msg_files_receive, out_dir, uorb_templates_dir,
|
||||
package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_id_map, uRTPS_CLIENT_TEMPL_FILE)
|
||||
|
||||
# Final steps to install client
|
||||
cp_wildcard(os.path.join(urtps_templates_dir,
|
||||
|
|
|
@ -41,6 +41,7 @@ precompiled and thus message generation will be much faster
|
|||
|
||||
import os
|
||||
import errno
|
||||
import yaml
|
||||
|
||||
import genmsg.msgs
|
||||
import gencpp
|
||||
|
@ -347,6 +348,37 @@ def print_field_def(field):
|
|||
array_size, comment))
|
||||
|
||||
|
||||
def get_absolute_path(arg_parse_dir):
|
||||
"""
|
||||
Get absolute path from 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
|
||||
|
||||
|
||||
def parse_yaml_msg_id_file(yaml_file):
|
||||
"""
|
||||
Parses a yaml file into a dict
|
||||
"""
|
||||
try:
|
||||
with open(yaml_file, 'r') as f:
|
||||
return yaml.load(f)
|
||||
except OSError as e:
|
||||
if e.errno == errno.ENOENT:
|
||||
raise IOError(errno.ENOENT, os.strerror(errno.ENOENT), yaml_file)
|
||||
else:
|
||||
raise
|
||||
|
||||
|
||||
def check_available_ids(used_msg_ids_list):
|
||||
"""
|
||||
Checks the available RTPS ID's
|
||||
|
@ -359,7 +391,13 @@ def rtps_message_id(msg_id_map, message):
|
|||
Get RTPS ID of uORB message
|
||||
"""
|
||||
used_ids = list()
|
||||
for dict in msg_id_map[0]['rtps']:
|
||||
# check 'send' list
|
||||
for dict in msg_id_map[0]['rtps']['send']:
|
||||
used_ids.append(dict['id'])
|
||||
if message in dict['msg']:
|
||||
return dict['id']
|
||||
# check 'receive' list
|
||||
for dict in msg_id_map[0]['rtps']['receive']:
|
||||
used_ids.append(dict['id'])
|
||||
if message in dict['msg']:
|
||||
return dict['id']
|
||||
|
|
|
@ -0,0 +1,168 @@
|
|||
#!/usr/bin/env python
|
||||
|
||||
################################################################################
|
||||
#
|
||||
# Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima).
|
||||
# 2018 PX4 Pro 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 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
|
||||
import os
|
||||
import argparse
|
||||
import px_generate_uorb_topic_helper
|
||||
|
||||
|
||||
class Classifier():
|
||||
"""
|
||||
Class to classify RTPS msgs as senders, receivers or to be ignored
|
||||
"""
|
||||
|
||||
def __init__(self, msg_id_map, msg_folder):
|
||||
self.msg_id_map = msg_id_map
|
||||
self.msg_folder = msg_folder
|
||||
self.msgs_to_send = {}
|
||||
self.msgs_to_receive = {}
|
||||
self.msgs_to_ignore = {}
|
||||
self.msg_files_send = []
|
||||
self.msg_files_receive = []
|
||||
self.msg_files_ignore = []
|
||||
|
||||
# init class attributtes
|
||||
self.set_msgs_to_send()
|
||||
self.set_msgs_to_receive()
|
||||
self.set_msgs_to_ignore()
|
||||
self.set_msg_files_send()
|
||||
self.set_msg_files_receive()
|
||||
self.set_msg_files_ignore()
|
||||
|
||||
# getters
|
||||
@property
|
||||
def msg_id_map(self):
|
||||
return self.__msg_id_map
|
||||
|
||||
@property
|
||||
def msg_folder(self):
|
||||
return self.__msg_folder
|
||||
|
||||
@property
|
||||
def msgs_to_send(self):
|
||||
return self.__msgs_to_send
|
||||
|
||||
@property
|
||||
def msgs_to_receive(self):
|
||||
return self.__msgs_to_receive
|
||||
|
||||
@property
|
||||
def msgs_to_ignore(self):
|
||||
return self.__msgs_to_ignore
|
||||
|
||||
@property
|
||||
def msg_files_send(self):
|
||||
return self.__msg_files_send
|
||||
|
||||
@property
|
||||
def msg_files_receive(self):
|
||||
return self.__msg_files_receive
|
||||
|
||||
@property
|
||||
def msg_files_ignore(self):
|
||||
return self.__msg_files_ignore
|
||||
|
||||
# setters (for class init)
|
||||
def set_msgs_to_send(self):
|
||||
for dict in self.msg_id_map['rtps']['send']:
|
||||
self.msgs_to_send.update({dict['msg']: dict['id']})
|
||||
|
||||
def set_msgs_to_receive(self):
|
||||
for dict in self.msg_id_map['rtps']['receive']:
|
||||
self.msgs_to_receive.update({dict['msg']: dict['id']})
|
||||
|
||||
def set_msgs_to_ignore(self):
|
||||
for dict in self.msg_id_map['rtps']['unclassified']:
|
||||
self.msgs_to_ignore.update({dict['msg']: dict['id']})
|
||||
|
||||
def set_msg_files_send(self):
|
||||
self.msgs_files_send = [px_generate_uorb_topic_helper.get_absolute_path(os.path.join(self.msg_folder, msg + ".msg"))
|
||||
for msg in self.msgs_to_send.keys()]
|
||||
|
||||
def set_msg_files_receive(self):
|
||||
self.msgs_files_receive = [px_generate_uorb_topic_helper.get_absolute_path(os.path.join(self.msg_folder, msg + ".msg"))
|
||||
for msg in self.msgs_to_receive.keys()]
|
||||
|
||||
def set_msg_files_ignore(self):
|
||||
self.msgs_files_ignore = [px_generate_uorb_topic_helper.get_absolute_path(os.path.join(self.msg_folder, msg + ".msg"))
|
||||
for msg in self.msgs_to_ignore.keys()]
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
parser = argparse.ArgumentParser()
|
||||
|
||||
parser.add_argument("-s", "--send", dest='send',
|
||||
action="store_true", help="Get topics to be sended")
|
||||
parser.add_argument("-r", "--receive", dest='receive',
|
||||
action="store_true", help="Get topics to be received")
|
||||
parser.add_argument("-i", "--ignore", dest='ignore',
|
||||
action="store_true", help="Get topics to be ignored")
|
||||
parser.add_argument("-p", "--path", dest='path',
|
||||
action="store_true", help="Get msgs and its paths")
|
||||
parser.add_argument("-m", "--topic-msg-dir", dest='msgdir', type=str,
|
||||
help="Topics message dir, by default msg/", default="msg")
|
||||
parser.add_argument("-y", "--rtps-ids-file", dest='yaml_file', type=str,
|
||||
help="RTPS msg IDs definition file, relative to the msg_dir, by default tools/uorb_rtps_message_ids.yaml",
|
||||
default="tools/uorb_rtps_message_ids.yaml")
|
||||
|
||||
# Parse arguments
|
||||
args = parser.parse_args()
|
||||
|
||||
msg_folder = os.path.dirname(os.path.dirname(os.path.abspath(args.msgdir)))
|
||||
classifier = Classifier(px_generate_uorb_topic_helper.parse_yaml_msg_id_file(
|
||||
os.path.join(msg_folder, args.yaml_file)), msg_folder)
|
||||
|
||||
if args.send:
|
||||
if args.path:
|
||||
print ('send files: ' + ', '.join(str(msg_file)
|
||||
for msg_file in classifier.msgs_files_send) + '\n')
|
||||
else:
|
||||
print ('send topics: ' + ', '.join(str(msg)
|
||||
for msg in classifier.msgs_to_send.keys()) + '\n')
|
||||
if args.receive:
|
||||
if args.path:
|
||||
print ('receive files: ' + ', '.join(str(msg_file)
|
||||
for msg_file in classifier.msgs_files_receive) + '\n')
|
||||
else:
|
||||
print ('receive topics: ' + ', '.join(str(msg)
|
||||
for msg in classifier.msgs_to_receive.keys()) + '\n')
|
||||
if args.ignore:
|
||||
if args.path:
|
||||
print ('ignore files: ' + ', '.join(str(msg_file)
|
||||
for msg_file in classifier.msgs_files_ignore) + '\n')
|
||||
else:
|
||||
print ('ignore topics: ' + ', '.join(str(msg)
|
||||
for msg in classifier.msgs_to_ignore.keys()) + '\n')
|
|
@ -1,248 +1,252 @@
|
|||
rtps:
|
||||
- msg: actuator_armed
|
||||
id: 0
|
||||
- msg: actuator_control
|
||||
id: 1
|
||||
- msg: actuator_direct
|
||||
id: 2
|
||||
- msg: actuator_output
|
||||
id: 3
|
||||
- msg: adc_report
|
||||
id: 4
|
||||
- msg: airspeed
|
||||
id: 5
|
||||
- msg: battery_status
|
||||
id: 6
|
||||
- msg: camera_capture
|
||||
id: 7
|
||||
- msg: camera_trigger
|
||||
id: 8
|
||||
- msg: collision_report
|
||||
id: 9
|
||||
- msg: commander_state
|
||||
id: 10
|
||||
- msg: cpuload
|
||||
id: 11
|
||||
- msg: debug_key_value
|
||||
id: 12
|
||||
- msg: debug_value
|
||||
id: 13
|
||||
- msg: debug_vect
|
||||
id: 14
|
||||
- msg: differential_pressure
|
||||
id: 15
|
||||
- msg: distance_sensor
|
||||
id: 16
|
||||
- msg: ekf2_innovations
|
||||
id: 17
|
||||
- msg: ekf2_timestamps
|
||||
id: 18
|
||||
- msg: ekf_gps_drift
|
||||
id: 19
|
||||
- msg: ekf_gps_position
|
||||
id: 20
|
||||
- msg: esc_report
|
||||
id: 21
|
||||
- msg: esc_status
|
||||
id: 22
|
||||
- msg: estimator_status
|
||||
id: 23
|
||||
- msg: follow_target
|
||||
id: 24
|
||||
- msg: geofence_result
|
||||
id: 25
|
||||
- msg: gps_dump
|
||||
id: 26
|
||||
- msg: gps_inject_data
|
||||
id: 27
|
||||
- msg: home_position
|
||||
id: 28
|
||||
- msg: input_rc
|
||||
id: 29
|
||||
- msg: iridiumsbd_status
|
||||
id: 30
|
||||
- msg: irlock_report
|
||||
id: 31
|
||||
- msg: landing_target_innovation
|
||||
id: 32
|
||||
- msg: landing_target_pose
|
||||
id: 33
|
||||
- msg: led_control
|
||||
id: 34
|
||||
- msg: log_message
|
||||
id: 35
|
||||
- msg: manual_control_setpoint
|
||||
id: 36
|
||||
- msg: mavlink_log
|
||||
id: 37
|
||||
- msg: mission
|
||||
id: 38
|
||||
- msg: mission_result
|
||||
id: 39
|
||||
- msg: mount_orientation
|
||||
id: 40
|
||||
- msg: multirotor_motor_limit
|
||||
id: 41
|
||||
- msg: obstacle_distance
|
||||
id: 42
|
||||
- msg: offboard_control_mode
|
||||
id: 43
|
||||
- msg: optical_flow
|
||||
id: 44
|
||||
- msg: parameter_update
|
||||
id: 45
|
||||
- msg: ping
|
||||
id: 46
|
||||
- msg: position_controller_landing_status
|
||||
id: 47
|
||||
- msg: position_controller_status
|
||||
id: 48
|
||||
- msg: position_setpoint
|
||||
id: 49
|
||||
- msg: position_setpoint_triplet
|
||||
id: 50
|
||||
- msg: power_button_state
|
||||
id: 51
|
||||
- msg: pwm_input
|
||||
id: 52
|
||||
- msg: qshell_req
|
||||
id: 53
|
||||
- msg: qshell_retval
|
||||
id: 54
|
||||
- msg: radio_status
|
||||
id: 55
|
||||
- msg: rate_ctrl_status
|
||||
id: 56
|
||||
- msg: rc_channels
|
||||
id: 57
|
||||
- msg: rc_parameter_map
|
||||
id: 58
|
||||
- msg: safety
|
||||
id: 59
|
||||
- msg: satellite_info
|
||||
id: 60
|
||||
- msg: sensor_accel
|
||||
id: 61
|
||||
- msg: sensor_baro
|
||||
id: 62
|
||||
- msg: sensor_bias
|
||||
id: 63
|
||||
- msg: sensor_combined
|
||||
id: 64
|
||||
- msg: sensor_correction
|
||||
id: 65
|
||||
- msg: sensor_gyro
|
||||
id: 66
|
||||
- msg: sensor_mag
|
||||
id: 67
|
||||
- msg: sensor_preflight
|
||||
id: 68
|
||||
- msg: sensor_selection
|
||||
id: 69
|
||||
- msg: servorail_status
|
||||
id: 70
|
||||
- msg: subsystem_info
|
||||
id: 71
|
||||
- msg: system_power
|
||||
id: 72
|
||||
- msg: task_stack_info
|
||||
id: 73
|
||||
- msg: tecs_status
|
||||
id: 74
|
||||
- msg: telemetry_status
|
||||
id: 75
|
||||
- msg: test_motor
|
||||
id: 76
|
||||
- msg: timesync_status
|
||||
id: 77
|
||||
- msg: trajectory_waypoint
|
||||
id: 78
|
||||
- msg: transponder_report
|
||||
id: 79
|
||||
- msg: tune_control
|
||||
id: 80
|
||||
- msg: uavcan_parameter_request
|
||||
id: 81
|
||||
- msg: uavcan_parameter_value
|
||||
id: 82
|
||||
- msg: ulog_stream
|
||||
id: 83
|
||||
- msg: ulog_stream_ack
|
||||
id: 84
|
||||
- msg: vehicle_air_data
|
||||
id: 85
|
||||
- msg: vehicle_attitude
|
||||
id: 86
|
||||
- msg: vehicle_attitude_setpoint
|
||||
id: 87
|
||||
- msg: vehicle_command
|
||||
id: 88
|
||||
- msg: vehicle_command_ack
|
||||
id: 89
|
||||
- msg: vehicle_constraint
|
||||
id: 90
|
||||
- msg: vehicle_control_mode
|
||||
id: 91
|
||||
- msg: vehicle_global_position
|
||||
id: 92
|
||||
- msg: vehicle_gps_position
|
||||
id: 93
|
||||
- msg: vehicle_land_detected
|
||||
id: 94
|
||||
- msg: vehicle_local_position
|
||||
id: 95
|
||||
- msg: vehicle_local_position_setpoint
|
||||
id: 96
|
||||
- msg: vehicle_magnetometer
|
||||
id: 97
|
||||
- msg: vehicle_odometry
|
||||
id: 98
|
||||
- msg: vehicle_rates_setpoint
|
||||
id: 99
|
||||
- msg: vehicle_roi
|
||||
id: 100
|
||||
- msg: vehicle_status
|
||||
id: 101
|
||||
- msg: vehicle_status_flags
|
||||
id: 102
|
||||
- msg: vehicle_trajectory_waypoint
|
||||
id: 103
|
||||
- msg: vtol_vehicle_status
|
||||
id: 104
|
||||
- msg: wind_estimate
|
||||
id: 105
|
||||
# multi topics
|
||||
- msg: actuator_control0
|
||||
id: 120
|
||||
- msg: actuator_control1
|
||||
id: 121
|
||||
- msg: actuator_control2
|
||||
id: 122
|
||||
- msg: actuator_control3
|
||||
id: 123
|
||||
- msg: actuator_controls_virtual_fw
|
||||
id: 124
|
||||
- msg: actuator_controls_virtual_mc
|
||||
id: 125
|
||||
- msg: mc_virtual_attitude_setpoint
|
||||
id: 126
|
||||
- msg: fw_virtual_attitude_setpoint
|
||||
id: 127
|
||||
- msg: vehicle_attitude_groundtruth
|
||||
id: 128
|
||||
- msg: vehicle_vision_attitude
|
||||
id: 129
|
||||
- msg: vehicle_global_position_groundtruth
|
||||
id: 130
|
||||
- msg: vehicle_local_position_groundtruth
|
||||
id: 131
|
||||
- msg: vehicle_mocap_odometry
|
||||
id: 132
|
||||
- msg: vehicle_visual_odometry
|
||||
id: 133
|
||||
- msg: mc_virtual_rates_setpoint
|
||||
id: 134
|
||||
- msg: fw_virtual_rates_setpoint
|
||||
id: 135
|
||||
- msg: vehicle_trajectory_waypoint_desired
|
||||
id: 136
|
||||
send:
|
||||
- msg: airspeed
|
||||
id: 5
|
||||
- msg: battery_status
|
||||
id: 6
|
||||
- msg: differential_pressure
|
||||
id: 15
|
||||
- msg: esc_status
|
||||
id: 22
|
||||
- msg: estimator_status
|
||||
id: 23
|
||||
- msg: iridiumsbd_status
|
||||
id: 30
|
||||
- msg: radio_status
|
||||
id: 55
|
||||
- msg: rate_ctrl_status
|
||||
id: 56
|
||||
- msg: satellite_info
|
||||
id: 60
|
||||
- msg: sensor_accel
|
||||
id: 61
|
||||
- msg: sensor_baro
|
||||
id: 62
|
||||
- msg: sensor_bias
|
||||
id: 63
|
||||
- msg: sensor_combined
|
||||
id: 64
|
||||
- msg: sensor_correction
|
||||
id: 65
|
||||
- msg: sensor_gyro
|
||||
id: 66
|
||||
- msg: sensor_mag
|
||||
id: 67
|
||||
- msg: sensor_preflight
|
||||
id: 68
|
||||
- msg: sensor_selection
|
||||
id: 69
|
||||
- msg: servorail_status
|
||||
id: 70
|
||||
- msg: subsystem_info
|
||||
id: 71
|
||||
- msg: tecs_status
|
||||
id: 74
|
||||
- msg: telemetry_status
|
||||
id: 75
|
||||
- msg: transponder_report
|
||||
id: 79
|
||||
- msg: vehicle_global_position
|
||||
id: 92
|
||||
- msg: vehicle_gps_position
|
||||
id: 93
|
||||
- msg: vehicle_land_detected
|
||||
id: 94
|
||||
- msg: vehicle_odometry
|
||||
id: 98
|
||||
- msg: vehicle_status
|
||||
id: 101
|
||||
- msg: vtol_vehicle_status
|
||||
id: 104
|
||||
receive:
|
||||
- msg: camera_trigger
|
||||
id: 8
|
||||
- msg: debug_key_value
|
||||
id: 12
|
||||
- msg: debug_value
|
||||
id: 13
|
||||
- msg: debug_vect
|
||||
id: 14
|
||||
- msg: obstacle_distance
|
||||
id: 42
|
||||
# multi topics
|
||||
- msg: vehicle_mocap_odometry
|
||||
id: 132
|
||||
- msg: vehicle_visual_odometry
|
||||
id: 133
|
||||
unclassified:
|
||||
- msg: actuator_armed
|
||||
id: 0
|
||||
- msg: actuator_control
|
||||
id: 1
|
||||
- msg: actuator_direct
|
||||
id: 2
|
||||
- msg: actuator_output
|
||||
id: 3
|
||||
- msg: adc_report
|
||||
id: 4
|
||||
- msg: camera_capture
|
||||
id: 7
|
||||
- msg: collision_report
|
||||
id: 9
|
||||
- msg: commander_state
|
||||
id: 10
|
||||
- msg: cpuload
|
||||
id: 11
|
||||
- msg: distance_sensor
|
||||
id: 16
|
||||
- msg: ekf2_innovations
|
||||
id: 17
|
||||
- msg: ekf2_timestamps
|
||||
id: 18
|
||||
- msg: ekf_gps_drift
|
||||
id: 19
|
||||
- msg: ekf_gps_position
|
||||
id: 20
|
||||
- msg: esc_report
|
||||
id: 21
|
||||
- msg: follow_target
|
||||
id: 24
|
||||
- msg: geofence_result
|
||||
id: 25
|
||||
- msg: gps_dump
|
||||
id: 26
|
||||
- msg: gps_inject_data
|
||||
id: 27
|
||||
- msg: home_position
|
||||
id: 28
|
||||
- msg: input_rc
|
||||
id: 29
|
||||
- msg: irlock_report
|
||||
id: 31
|
||||
- msg: landing_target_innovation
|
||||
id: 32
|
||||
- msg: landing_target_pose
|
||||
id: 33
|
||||
- msg: led_control
|
||||
id: 34
|
||||
- msg: log_message
|
||||
id: 35
|
||||
- msg: manual_control_setpoint
|
||||
id: 36
|
||||
- msg: mavlink_log
|
||||
id: 37
|
||||
- msg: mission
|
||||
id: 38
|
||||
- msg: mission_result
|
||||
id: 39
|
||||
- msg: mount_orientation
|
||||
id: 40
|
||||
- msg: multirotor_motor_limit
|
||||
id: 41
|
||||
- msg: offboard_control_mode
|
||||
id: 43
|
||||
- msg: optical_flow
|
||||
id: 44
|
||||
- msg: parameter_update
|
||||
id: 45
|
||||
- msg: ping
|
||||
id: 46
|
||||
- msg: position_controller_landing_status
|
||||
id: 47
|
||||
- msg: position_controller_status
|
||||
id: 48
|
||||
- msg: position_setpoint
|
||||
id: 49
|
||||
- msg: position_setpoint_triplet
|
||||
id: 50
|
||||
- msg: power_button_state
|
||||
id: 51
|
||||
- msg: pwm_input
|
||||
id: 52
|
||||
- msg: qshell_req
|
||||
id: 53
|
||||
- msg: qshell_retval
|
||||
id: 54
|
||||
- msg: rc_channels
|
||||
id: 57
|
||||
- msg: rc_parameter_map
|
||||
id: 58
|
||||
- msg: safety
|
||||
id: 59
|
||||
- msg: system_power
|
||||
id: 72
|
||||
- msg: task_stack_info
|
||||
id: 73
|
||||
- msg: test_motor
|
||||
id: 76
|
||||
- msg: timesync_status
|
||||
id: 77
|
||||
- msg: trajectory_waypoint
|
||||
id: 78
|
||||
- msg: tune_control
|
||||
id: 80
|
||||
- msg: uavcan_parameter_request
|
||||
id: 81
|
||||
- msg: uavcan_parameter_value
|
||||
id: 82
|
||||
- msg: ulog_stream
|
||||
id: 83
|
||||
- msg: ulog_stream_ack
|
||||
id: 84
|
||||
- msg: vehicle_air_data
|
||||
id: 85
|
||||
- msg: vehicle_attitude
|
||||
id: 86
|
||||
- msg: vehicle_attitude_setpoint
|
||||
id: 87
|
||||
- msg: vehicle_command
|
||||
id: 88
|
||||
- msg: vehicle_command_ack
|
||||
id: 89
|
||||
- msg: vehicle_constraint
|
||||
id: 90
|
||||
- msg: vehicle_control_mode
|
||||
id: 91
|
||||
- msg: vehicle_local_position
|
||||
id: 95
|
||||
- msg: vehicle_local_position_setpoint
|
||||
id: 96
|
||||
- msg: vehicle_magnetometer
|
||||
id: 97
|
||||
- msg: vehicle_rates_setpoint
|
||||
id: 99
|
||||
- msg: vehicle_roi
|
||||
id: 100
|
||||
- msg: vehicle_status_flags
|
||||
id: 102
|
||||
- msg: vehicle_trajectory_waypoint
|
||||
id: 103
|
||||
- msg: wind_estimate
|
||||
id: 105
|
||||
# multi topics
|
||||
- msg: actuator_control0
|
||||
id: 120
|
||||
- msg: actuator_control1
|
||||
id: 121
|
||||
- msg: actuator_control2
|
||||
id: 122
|
||||
- msg: actuator_control3
|
||||
id: 123
|
||||
- msg: actuator_controls_virtual_fw
|
||||
id: 124
|
||||
- msg: actuator_controls_virtual_mc
|
||||
id: 125
|
||||
- msg: mc_virtual_attitude_setpoint
|
||||
id: 126
|
||||
- msg: fw_virtual_attitude_setpoint
|
||||
id: 127
|
||||
- msg: vehicle_attitude_groundtruth
|
||||
id: 128
|
||||
- msg: vehicle_vision_attitude
|
||||
id: 129
|
||||
- msg: vehicle_global_position_groundtruth
|
||||
id: 130
|
||||
- msg: vehicle_local_position_groundtruth
|
||||
id: 131
|
||||
- msg: mc_virtual_rates_setpoint
|
||||
id: 134
|
||||
- msg: fw_virtual_rates_setpoint
|
||||
id: 135
|
||||
- msg: vehicle_trajectory_waypoint_desired
|
||||
id: 136
|
||||
|
|
Loading…
Reference in New Issue