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

microRTPS_bridge: structural change to make possible to generate pub/sub code for multi-topics/aliased msgs

This commit is contained in:
TSC21 2019-06-02 16:12:50 +01:00 committed by Beat Küng
parent a747116eab
commit 3d9f83a4f2
23 changed files with 290 additions and 153 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Tools/jMAVSim

@ -1 +1 @@
Subproject commit def7501bc0536b8d1050f65d09c7dfbebcc0ce61
Subproject commit dc10a13c78afeb97f7b570f8049ec8a912e22d81

2
Tools/sitl_gazebo

@ -1 +1 @@
Subproject commit 22df9475ca0d157e2db066a20f64c35906bf7f25
Subproject commit a0b0eb8df12b11e8f63d3cc766dee97cefa32ab7

2
mavlink/include/mavlink/v2.0

@ -1 +1 @@
Subproject commit 18cf6ff2fc0e51e4555b19fc31e8b06eb38bdd79
Subproject commit 60df2613725c21f3eedc51251285f80a22e7d07d

21
msg/templates/uorb_microcdr/microRTPS_client.cpp.em
View File

@ -18,9 +18,11 @@ 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/
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]
topic_names = [s.short_name for idx, s in enumerate(spec)]
send_topics = [(alias[idx] if alias[idx] else s.short_name) for idx, s in enumerate(spec) if scope[idx] == MsgScope.SEND]
send_base_types = [s.short_name for idx, s in enumerate(spec) if scope[idx] == MsgScope.SEND]
recv_topics = [(alias[idx] if alias[idx] else s.short_name) for idx, s in enumerate(spec) if scope[idx] == MsgScope.RECEIVE]
receive_base_types = [s.short_name for idx, s in enumerate(spec) if scope[idx] == MsgScope.RECEIVE]
}@
/****************************************************************************
*
@ -108,11 +110,11 @@ void* send(void* /*unused*/)
if (updated)
{
// obtained data for the file descriptor
struct @(topic)_s data;
struct @(send_base_types[idx])_s data;
// copy raw data into local buffer
if (orb_copy(ORB_ID(@(topic)), fds[@(idx)], &data) == 0) {
/* payload is shifted by header length to make room for header*/
serialize_@(topic)(&writer, &data, &data_buffer[header_length], &length);
serialize_@(send_base_types[idx])(&writer, &data, &data_buffer[header_length], &length);
if (0 < (read = transport_node->write((char)@(rtps_message_id(ids, topic)), data_buffer, length)))
{
@ -161,8 +163,8 @@ void micrortps_start_topics(struct timespec &begin, int &total_read, uint32_t &r
uint8_t topic_ID = 255;
// Declare received topics
@[for topic in recv_topics]@
struct @(topic)_s @(topic)_data;
@[for idx, topic in enumerate(recv_topics)]@
struct @(receive_base_types[idx])_s @(topic)_data;
orb_advert_t @(topic)_pub = nullptr;
@[end for]@
@ -188,11 +190,10 @@ void micrortps_start_topics(struct timespec &begin, int &total_read, uint32_t &r
total_read += read;
switch (topic_ID)
{
@[for topic in recv_topics]@
@[for idx, topic in enumerate(recv_topics)]@
case @(rtps_message_id(ids, topic)):
{
deserialize_@(topic)(&reader, &@(topic)_data, data_buffer);
deserialize_@(receive_base_types[idx])(&reader, &@(topic)_data, data_buffer);
if (!@(topic)_pub) {
@(topic)_pub = orb_advertise(ORB_ID(@(topic)), &@(topic)_data);
} else {

3
msg/templates/urtps/Publisher.cpp.em
View File

@ -15,11 +15,12 @@ import genmsg.msgs
import gencpp
from px_generate_uorb_topic_helper import * # this is in Tools/
topic = spec.short_name
topic = alias if alias else spec.short_name
}@
/****************************************************************************
*
* Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima).
* Copyright (C) 2018-2019 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:

3
msg/templates/urtps/Publisher.h.em
View File

@ -15,11 +15,12 @@ import genmsg.msgs
import gencpp
from px_generate_uorb_topic_helper import * # this is in Tools/
topic = spec.short_name
topic = alias if alias else spec.short_name
}@
/****************************************************************************
*
* Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima).
* Copyright (C) 2018-2019 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:

7
msg/templates/urtps/RtpsTopics.cpp.em
View File

@ -17,12 +17,13 @@ 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/
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]
send_topics = [(alias[idx] if alias[idx] else s.short_name) for idx, s in enumerate(spec) if scope[idx] == MsgScope.SEND]
recv_topics = [(alias[idx] if alias[idx] else s.short_name) for idx, s in enumerate(spec) if scope[idx] == MsgScope.RECEIVE]
}@
/****************************************************************************
*
* Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima).
* Copyright (C) 2018-2019 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:
@ -88,7 +89,7 @@ void RtpsTopics::publish(uint8_t topic_ID, char data_buffer[], size_t len)
{
switch (topic_ID)
{
@[for topic in send_topics]@
@[for idx, topic in enumerate(send_topics)]@
case @(rtps_message_id(ids, topic)): // @(topic)
{
@(topic)_ st;

6
msg/templates/urtps/RtpsTopics.h.em
View File

@ -17,13 +17,13 @@ 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/
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]
send_topics = [(alias[idx] if alias[idx] else s.short_name) for idx, s in enumerate(spec) if scope[idx] == MsgScope.SEND]
recv_topics = [(alias[idx] if alias[idx] else s.short_name) for idx, s in enumerate(spec) if scope[idx] == MsgScope.RECEIVE]
}@
/****************************************************************************
*
* Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima).
* Copyright (C) 2018-2019 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:

3
msg/templates/urtps/Subscriber.cpp.em
View File

@ -15,11 +15,12 @@ import genmsg.msgs
import gencpp
from px_generate_uorb_topic_helper import * # this is in Tools/
topic = spec.short_name
topic = alias if alias else spec.short_name
}@
/****************************************************************************
*
* Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima).
* Copyright (C) 2018-2019 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:

3
msg/templates/urtps/Subscriber.h.em
View File

@ -15,11 +15,12 @@ import genmsg.msgs
import gencpp
from px_generate_uorb_topic_helper import * # this is in Tools/
topic = spec.short_name
topic = alias if alias else spec.short_name
}@
/****************************************************************************
*
* Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima).
* Copyright (C) 2018-2019 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:

5
msg/templates/urtps/microRTPS_agent.cpp.em
View File

@ -16,9 +16,8 @@ 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/
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]
send_topics = [(alias[idx] if alias[idx] else s.short_name) for idx, s in enumerate(spec) if scope[idx] == MsgScope.SEND]
recv_topics = [(alias[idx] if alias[idx] else s.short_name) for idx, s in enumerate(spec) if scope[idx] == MsgScope.RECEIVE]
}@
/****************************************************************************
*

1
msg/templates/urtps/microRTPS_agent_CMakeLists.txt.em
View File

@ -1,6 +1,7 @@
################################################################################
#
# Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima).
# Copyright (C) 2018-2019 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
msg/templates/urtps/microRTPS_transport.h
View File

@ -1,6 +1,7 @@
/****************************************************************************
*
* Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima).
* Copyright (C) 2018-2019 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:

21
msg/templates/urtps/msg.idl.em
View File

@ -7,6 +7,7 @@
@################################################################################
@#
@# Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima).
@# Copyright (C) 2018-2019 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:
@ -42,6 +43,8 @@ from px_generate_uorb_topic_helper import * # this is in Tools/
builtin_types = set()
array_types = set()
topic = alias if alias else spec.short_name
}@
@#################################################
@# Searching for serialize function per each field
@ -71,7 +74,7 @@ def get_idl_type_name(field_type):
def add_msg_field(field):
if (not field.is_header):
if field.is_array:
print(' {0}__{1}_array_{2} {3}_;'.format(spec.short_name, str(get_idl_type_name(field.base_type)).replace(" ", "_"), str(field.array_len), field.name))
print(' {0}__{1}_array_{2} {3}_;'.format(topic, str(get_idl_type_name(field.base_type)).replace(" ", "_"), str(field.array_len), field.name))
else:
base_type = get_idl_type_name(field.base_type) + "_" if get_idl_type_name(field.base_type) in builtin_types else get_idl_type_name(field.base_type)
print(' {0} {1}_;'.format(base_type, field.name))
@ -86,7 +89,7 @@ def add_array_typedefs():
for field in spec.parsed_fields():
if not field.is_header and field.is_array:
base_type = get_idl_type_name(field.base_type) + "_" if get_idl_type_name(field.base_type) in builtin_types else get_idl_type_name(field.base_type)
array_type = 'typedef {0} {1}__{2}_array_{3}[{4}];'.format(base_type, spec.short_name, get_idl_type_name(field.base_type).replace(" ", "_"), field.array_len, field.array_len)
array_type = 'typedef {0} {1}__{2}_array_{3}[{4}];'.format(base_type, topic, get_idl_type_name(field.base_type).replace(" ", "_"), field.array_len, field.array_len)
if array_type not in array_types:
array_types.add(array_type)
for atype in array_types:
@ -97,11 +100,11 @@ def add_msg_constants():
sorted_constants = sorted(spec.constants,
key=sizeof_field_type, reverse=True)
for constant in sorted_constants:
print('const {0} {1}__{2} = {3};'.format(get_idl_type_name(constant.type), spec.short_name, constant.name, constant.val))
print('const {0} {1}__{2} = {3};'.format(get_idl_type_name(constant.type), topic, constant.name, constant.val))
}
#ifndef __@(spec.short_name)__idl__
#define __@(spec.short_name)__idl__
#ifndef __@(topic)__idl__
#define __@(topic)__idl__
@#############################
@# Include dependency messages
@ -113,11 +116,11 @@ def add_msg_constants():
@add_msg_constants()
@# Array types
@add_array_typedefs()
struct @(spec.short_name)_
struct @(topic)_
{
@add_msg_fields()
}; // struct @(spec.short_name)_
}; // struct @(topic)_
#pragma keylist @(spec.short_name)_
#pragma keylist @(topic)_
#endif // __@(spec.short_name)__idl__
#endif // __@(topic)__idl__

125
msg/tools/generate_microRTPS_bridge.py
View File

@ -3,7 +3,7 @@
################################################################################
#
# Copyright 2017 Proyectos y Sistemas de Mantenimiento SL (eProsima).
# 2018 PX4 Pro Development Team. All rights reserved.
# 2018-2019 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:
@ -62,61 +62,66 @@ def check_rtps_id_uniqueness(classifier):
repeated_ids = dict()
full_send_list = dict(list(msg for msg in classifier.msgs_to_send.items()) + list(msg[0].items()[0] for msg in classifier.alias_msgs_to_send))
full_receive_list = dict(list(msg for msg in classifier.msgs_to_receive.items()) + list(msg[0].items()[0] for msg in classifier.alias_msgs_to_receive))
full_ignore_list = dict(list(msg for msg in classifier.msgs_to_ignore.items()) + list(msg[0].items()[0] for msg in classifier.alias_msgs_to_ignore))
# check if there are repeated ID's on the messages to send
for key, value in classifier.msgs_to_send.items():
for key, value in full_send_list.items():
if sys.version_info[0] < 3:
if classifier.msgs_to_send.values().count(value) > 1:
if full_send_list.values().count(value) > 1:
repeated_ids.update({key: value})
else:
if list(classifier.msgs_to_send.values()).count(value) > 1:
if list(full_send_list.values()).count(value) > 1:
repeated_ids.update({key: value})
# check if there are repeated ID's on the messages to receive
for key, value in classifier.msgs_to_receive.items():
for key, value in full_receive_list.items():
if sys.version_info[0] < 3:
if classifier.msgs_to_receive.values().count(value) > 1:
if full_receive_list.values().count(value) > 1:
repeated_ids.update({key: value})
else:
if list(classifier.msgs_to_receive.values()).count(value) > 1:
if list(full_receive_list.values()).count(value) > 1:
repeated_ids.update({key: value})
# check if there are repeated ID's on the messages to ignore
for key, value in classifier.msgs_to_ignore.items():
for key, value in full_ignore_list.items():
if sys.version_info[0] < 3:
if classifier.msgs_to_ignore.values().count(value) > 1:
if full_ignore_list.values().count(value) > 1:
repeated_ids.update({key: value})
else:
if list(classifier.msgs_to_ignore.values()).count(value) > 1:
if list(full_ignore_list.values()).count(value) > 1:
repeated_ids.update({key: value})
# check if there are repeated IDs between classified and unclassified msgs
# check send and ignore lists
send_ignore_common_ids = list(set(classifier.msgs_to_ignore.values(
)).intersection(classifier.msgs_to_send.values()))
for item in zip(classifier.msgs_to_send.items(), classifier.msgs_to_ignore.items()):
send_ignore_common_ids = list(set(full_ignore_list.values(
)).intersection(full_send_list.values()))
for item in full_send_list.items():
for repeated in send_ignore_common_ids:
if item[1] == repeated:
repeated_ids.update({item[0]: item[1]})
for item in classifier.msgs_to_ignore.items():
for item in full_ignore_list.items():
for repeated in send_ignore_common_ids:
if item[1] == repeated:
repeated_ids.update({item[0]: item[1]})
# check receive and ignore lists
receive_ignore_common_ids = list(set(classifier.msgs_to_ignore.values(
)).intersection(classifier.msgs_to_receive.values()))
for item in classifier.msgs_to_receive.items():
receive_ignore_common_ids = list(set(full_ignore_list.values(
)).intersection(full_receive_list.values()))
for item in full_receive_list.items():
for repeated in receive_ignore_common_ids:
if item[1] == repeated:
repeated_ids.update({item[0]: item[1]})
for item in classifier.msgs_to_ignore.items():
for item in full_ignore_list.items():
for repeated in receive_ignore_common_ids:
if item[1] == repeated:
repeated_ids.update({item[0]: item[1]})
all_msgs = classifier.msgs_to_send
all_msgs.update(classifier.msgs_to_receive)
all_msgs.update(classifier.msgs_to_ignore)
all_msgs = {}
all_msgs.update(full_send_list)
all_msgs.update(full_receive_list)
all_msgs.update(full_ignore_list)
all_ids = list()
if sys.version_info[0] < 3:
all_ids = all_msgs.values()
@ -277,44 +282,76 @@ uRTPS_SUBSCRIBER_H_TEMPL_FILE = 'Subscriber.h.em'
def generate_agent(out_dir):
if classifier.msg_files_send:
for msg_file in classifier.msg_files_send:
# raise Exception(classifier.msgs_to_receive)
if classifier.msgs_to_send:
for msg_file in classifier.msgs_to_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,
px_generate_uorb_topic_files.generate_idl_file(msg_file, msg_dir, "", os.path.join(out_dir, "/idl"), urtps_templates_dir,
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,
px_generate_uorb_topic_files.generate_idl_file(msg_file, msg_dir, "", idl_dir, urtps_templates_dir,
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,
px_generate_uorb_topic_files.generate_topic_file(msg_file, msg_dir, "", out_dir, urtps_templates_dir,
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,
px_generate_uorb_topic_files.generate_topic_file(msg_file, msg_dir, "", out_dir, urtps_templates_dir,
package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_id_map, uRTPS_PUBLISHER_H_TEMPL_FILE)
if classifier.msg_files_receive:
for msg_file in classifier.msg_files_receive:
if classifier.alias_msgs_to_send:
for msg_file in classifier.alias_msgs_to_send:
msg_alias = msg_file[0].keys()[0]
msg_name = msg_file[1]
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,
px_generate_uorb_topic_files.generate_idl_file(msg_name, msg_dir, msg_alias, os.path.join(out_dir, "/idl"), urtps_templates_dir,
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,
px_generate_uorb_topic_files.generate_idl_file(msg_name, msg_dir, msg_alias, idl_dir, urtps_templates_dir,
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,
px_generate_uorb_topic_files.generate_topic_file(msg_name, msg_dir, msg_alias, out_dir, urtps_templates_dir,
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_name, msg_dir, msg_alias, out_dir, urtps_templates_dir,
package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_id_map, uRTPS_PUBLISHER_H_TEMPL_FILE)
if classifier.msgs_to_receive:
for msg_file in classifier.msgs_to_receive:
if gen_idl:
if out_dir != agent_out_dir:
px_generate_uorb_topic_files.generate_idl_file(msg_file, msg_dir, "", os.path.join(out_dir, "/idl"), urtps_templates_dir,
package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_id_map)
else:
px_generate_uorb_topic_files.generate_idl_file(msg_file, msg_dir, "", idl_dir, urtps_templates_dir,
package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_id_map)
px_generate_uorb_topic_files.generate_topic_file(msg_file, msg_dir, "", out_dir, urtps_templates_dir,
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,
px_generate_uorb_topic_files.generate_topic_file(msg_file, msg_dir, "", out_dir, urtps_templates_dir,
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(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 classifier.alias_msgs_to_receive:
for msg_file in classifier.alias_msgs_to_receive:
msg_alias = msg_file[0].keys()[0]
msg_name = msg_file[1]
if gen_idl:
if out_dir != agent_out_dir:
px_generate_uorb_topic_files.generate_idl_file(msg_name, msg_dir, msg_alias, os.path.join(out_dir, "/idl"), urtps_templates_dir,
package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_id_map)
else:
px_generate_uorb_topic_files.generate_idl_file(msg_name, msg_dir, msg_alias, idl_dir, urtps_templates_dir,
package, px_generate_uorb_topic_files.INCL_DEFAULT, classifier.msg_id_map)
px_generate_uorb_topic_files.generate_topic_file(msg_name, msg_dir, msg_alias, out_dir, urtps_templates_dir,
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_name, msg_dir, msg_alias, out_dir, urtps_templates_dir,
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(classifier.msgs_to_send, classifier.alias_msgs_to_send, classifier.msgs_to_receive, classifier.alias_msgs_to_receive, msg_dir, 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.msgs_to_send, classifier.alias_msgs_to_send, classifier.msgs_to_receive, classifier.alias_msgs_to_receive, msg_dir, 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.msgs_to_send, classifier.alias_msgs_to_send, classifier.msgs_to_receive, classifier.alias_msgs_to_receive, msg_dir, 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(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)
px_generate_uorb_topic_files.generate_uRTPS_general(classifier.msgs_to_send, classifier.alias_msgs_to_send, classifier.msgs_to_receive, classifier.alias_msgs_to_receive, msg_dir, 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"))
@ -387,8 +424,8 @@ def generate_client(out_dir):
if os.path.isfile(def_file):
os.rename(def_file, def_file.replace(".h", ".h_"))
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)
px_generate_uorb_topic_files.generate_uRTPS_general(classifier.msgs_to_send, classifier.alias_msgs_to_send, classifier.msgs_to_receive, classifier.alias_msgs_to_receive, msg_dir,
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,

79
msg/tools/px_generate_uorb_topic_files.py
View File

@ -170,13 +170,24 @@ def generate_output_from_file(format_idx, filename, outputdir, package, template
return generate_by_template(output_file, template_file, em_globals)
def generate_idl_file(filename_msg, outputdir, templatedir, package, includepath, ids):
def generate_idl_file(filename_msg, msg_dir, alias, outputdir, templatedir, package, includepath, ids):
"""
Generates an .idl from .msg file
"""
em_globals = get_em_globals(
filename_msg, package, includepath, ids, MsgScope.NONE)
spec_short_name = em_globals["spec"].short_name
# Make sure input msg directory exists:
if not os.path.isdir(msg_dir):
os.makedirs(msg_dir)
msg = os.path.join(msg_dir, filename_msg + ".msg")
if (alias != ""):
em_globals = get_em_globals(
msg, alias , package, includepath, ids, MsgScope.NONE)
spec_short_name = alias
else:
em_globals = get_em_globals(
msg, "", package, includepath, ids, MsgScope.NONE)
spec_short_name = em_globals["spec"].short_name
# Make sure output directory exists:
if not os.path.isdir(outputdir):
@ -189,21 +200,39 @@ def generate_idl_file(filename_msg, outputdir, templatedir, package, includepath
return generate_by_template(output_file, template_file, em_globals)
def generate_uRTPS_general(filename_send_msgs, filename_received_msgs,
outputdir, templatedir, package, includepath, ids, template_name):
def generate_uRTPS_general(filename_send_msgs, filename_alias_send_msgs, filename_receive_msgs, filename_alias_receive_msgs,
msg_dir, outputdir, templatedir, package, includepath, ids, template_name):
"""
Generates source file by msg content
"""
em_globals_list = []
if filename_send_msgs:
em_globals_list.extend([get_em_globals(
f, package, includepath, ids, MsgScope.SEND) for f in filename_send_msgs])
# Make sure input msg directory exists:
if not os.path.isdir(msg_dir):
os.makedirs(msg_dir)
if filename_received_msgs:
send_msgs = list(os.path.join(msg_dir, msg + ".msg") for msg in filename_send_msgs)
alias_send_msgs = list([os.path.join(msg_dir, msg[1] + ".msg"), msg[0].keys()[0]] for msg in filename_alias_send_msgs)
receive_msgs = list(os.path.join(msg_dir, msg + ".msg") for msg in filename_receive_msgs)
alias_receive_msgs = list([os.path.join(msg_dir, msg[1] + ".msg"), msg[0].keys()[0]] for msg in filename_alias_receive_msgs)
em_globals_list = []
if send_msgs:
em_globals_list.extend([get_em_globals(
f, package, includepath, ids, MsgScope.RECEIVE) for f in filename_received_msgs])
f, "", package, includepath, ids, MsgScope.SEND) for f in send_msgs])
if alias_send_msgs:
em_globals_list.extend([get_em_globals(
f[0], f[1], package, includepath, ids, MsgScope.SEND) for f in alias_send_msgs])
if receive_msgs:
em_globals_list.extend([get_em_globals(
f, "", package, includepath, ids, MsgScope.RECEIVE) for f in receive_msgs])
if alias_receive_msgs:
em_globals_list.extend([get_em_globals(
f[0], f[1], package, includepath, ids, MsgScope.RECEIVE) for f in alias_receive_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)
@ -215,13 +244,24 @@ def generate_uRTPS_general(filename_send_msgs, filename_received_msgs,
return generate_by_template(output_file, template_file, merged_em_globals)
def generate_topic_file(filename_msg, outputdir, templatedir, package, includepath, ids, template_name):
def generate_topic_file(filename_msg, msg_dir, alias, outputdir, templatedir, package, includepath, ids, template_name):
"""
Generates an .idl from .msg file
Generates a sources and headers from .msg file
"""
em_globals = get_em_globals(
filename_msg, package, includepath, ids, MsgScope.NONE)
spec_short_name = em_globals["spec"].short_name
# Make sure input msg directory exists:
if not os.path.isdir(msg_dir):
os.makedirs(msg_dir)
msg = os.path.join(msg_dir, filename_msg + ".msg")
if (alias):
em_globals = get_em_globals(
msg, alias, package, includepath, ids, MsgScope.NONE)
spec_short_name = alias
else:
em_globals = get_em_globals(
msg, "", package, includepath, ids, MsgScope.NONE)
spec_short_name = em_globals["spec"].short_name
# Make sure output directory exists:
if not os.path.isdir(outputdir):
@ -234,7 +274,7 @@ def generate_topic_file(filename_msg, outputdir, templatedir, package, includepa
return generate_by_template(output_file, template_file, em_globals)
def get_em_globals(filename_msg, package, includepath, ids, scope):
def get_em_globals(filename_msg, alias, package, includepath, ids, scope):
"""
Generates em globals dictionary
"""
@ -261,7 +301,8 @@ def get_em_globals(filename_msg, package, includepath, ids, scope):
"topics": topics,
"ids": ids,
"scope": scope,
"package": package
"package": package,
"alias": alias
}
return em_globals

2
msg/tools/px_generate_uorb_topic_helper.py
View File

@ -1,7 +1,7 @@
#!/usr/bin/env python
#############################################################################
#
# Copyright (C) 2013-2018 PX4 Pro Development Team. All rights reserved.
# Copyright (C) 2013-2019 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

63
msg/tools/uorb_rtps_classifier.py
View File

@ -50,9 +50,9 @@ class Classifier():
self.msg_id_map = self.parse_yaml_msg_id_file(yaml_file)
self.check_if_listed(yaml_file)
self.msgs_to_send = self.set_msgs_to_send()
self.msgs_to_receive = self.set_msgs_to_receive()
self.msgs_to_ignore = self.set_msgs_to_ignore()
self.msgs_to_send, self.alias_msgs_to_send = self.set_msgs_to_send()
self.msgs_to_receive, self.alias_msgs_to_receive = self.set_msgs_to_receive()
self.msgs_to_ignore, self.alias_msgs_to_ignore = self.set_msgs_to_ignore()
self.msg_files_send = self.set_msg_files_send()
self.msg_files_receive = self.set_msg_files_receive()
self.msg_files_ignore = self.set_msg_files_ignore()
@ -67,24 +67,36 @@ class Classifier():
def set_msgs_to_send(self):
send = {}
send_alias = []
for dict in self.msg_id_map['rtps']:
if 'send' in dict.keys():
send.update({dict['msg']: dict['id']})
return send
if 'alias' in dict.keys():
send_alias.append(({dict['msg']: dict['id']}, dict['alias']))
else:
send.update({dict['msg']: dict['id']})
return send, send_alias
def set_msgs_to_receive(self):
receive = {}
receive_alias = []
for dict in self.msg_id_map['rtps']:
if 'receive' in dict.keys():
receive.update({dict['msg']: dict['id']})
return receive
if 'alias' in dict.keys():
receive_alias.append(({dict['msg']: dict['id']}, dict['alias']))
else:
receive.update({dict['msg']: dict['id']})
return receive, receive_alias
def set_msgs_to_ignore(self):
ignore = {}
ignore_alias = []
for dict in self.msg_id_map['rtps']:
if ('send' not in dict.keys()) and ('receive' not in dict.keys()):
ignore.update({dict['msg']: dict['id']})
return ignore
if (('send' not in dict.keys()) and ('receive' not in dict.keys())):
if 'alias' in dict.keys():
ignore_alias.append(({dict['msg']: dict['id']}, dict['alias']))
else:
ignore.update({dict['msg']: dict['id']})
return ignore, ignore_alias
def set_msg_files_send(self):
return [os.path.join(self.msg_folder, msg + ".msg")
@ -141,7 +153,9 @@ if __name__ == "__main__":
parser = argparse.ArgumentParser()
parser.add_argument("-s", "--send", dest='send',
action="store_true", help="Get topics to be sended")
action="store_true", help="Get topics to be sent")
parser.add_argument("-a", "--alias", dest='alias',
action="store_true", help="Get alias topics")
parser.add_argument("-r", "--receive", dest='receive',
action="store_true", help="Get topics to be received")
parser.add_argument("-i", "--ignore", dest='ignore',
@ -170,19 +184,34 @@ if __name__ == "__main__":
print ('send files: ' + ', '.join(str(msg_file)
for msg_file in classifier.msgs_files_send) + '\n')
else:
print (', '.join(str(msg)
for msg in classifier.msgs_to_send.keys()) + '\n')
if args.alias:
print (', '.join(str(msg)
for msg in classifier.msgs_to_send.keys()) + (' alias ' + ', '.join(str(msg[0].keys()[0])
for msg in classifier.alias_msgs_to_send) if len(classifier.alias_msgs_to_send) > 0 else '') + '\n')
else:
print (', '.join(str(msg)
for msg in classifier.msgs_to_send.keys()))
if args.receive:
if args.path:
print ('receive files: ' + ', '.join(str(msg_file)
for msg_file in classifier.msgs_files_receive) + '\n')
else:
print (', '.join(str(msg)
for msg in classifier.msgs_to_receive.keys()) + '\n')
if args.alias:
print (', '.join(str(msg)
for msg in classifier.msgs_to_receive.keys()) + (' alias ' + ', '.join(str(msg[0].keys()[0])
for msg in classifier.alias_msgs_to_receive) if len(classifier.alias_msgs_to_receive) > 0 else '') + '\n')
else:
print (', '.join(str(msg)
for msg in classifier.msgs_to_receive.keys()))
if args.ignore:
if args.path:
print ('ignore files: ' + ', '.join(str(msg_file)
for msg_file in classifier.msgs_files_ignore) + '\n')
else:
print (', '.join(str(msg)
for msg in classifier.msgs_to_ignore.keys()) + '\n')
if args.alias:
print (', '.join(str(msg)
for msg in classifier.msgs_to_ignore.keys()) + (' alias ' + ', '.join(str(msg[0].keys()[0])
for msg in classifier.alias_msgs_to_ignore) if len(classifier.alias_msgs_to_ignore) > 0 else '') + '\n')
else:
print (', '.join(str(msg)
for msg in classifier.msgs_to_ignore.keys()))

36
msg/tools/uorb_rtps_message_ids.yaml
View File

@ -98,7 +98,7 @@ rtps:
id: 40
- msg: mount_orientation
id: 41
- msg: multirotor_motor_limit
- msg: multirotor_motor_limits
id: 42
- msg: obstacle_distance
id: 43
@ -207,7 +207,7 @@ rtps:
id: 89
- msg: vehicle_command_ack
id: 90
- msg: vehicle_constraint
- msg: vehicle_constraints
id: 91
- msg: vehicle_control_mode
id: 92
@ -246,46 +246,54 @@ rtps:
- msg: collision_constraints
id: 107
send: true
- msg: multirotor_motor_limits
id: 108
- msg: vehicle_constraints
id: 109
- msg: orbit_status
id: 110
id: 108
- msg: power_monitor
id: 111
id: 109
- msg: landing_gear
id: 112
id: 110
# multi topics
- msg: actuator_controls_0
id: 120
alias: actuactor_controls
- msg: actuator_controls_1
id: 121
alias: actuactor_controls
- msg: actuator_controls_2
id: 122
alias: actuactor_controls
- msg: actuator_controls_3
id: 123
alias: actuactor_controls
- msg: actuator_controls_virtual_fw
id: 124
alias: actuactor_controls
- msg: actuator_controls_virtual_mc
id: 125
alias: actuactor_controls
- msg: mc_virtual_attitude_setpoint
id: 126
alias: vehicle_attitude_setpoint
- msg: fw_virtual_attitude_setpoint
id: 127
alias: vehicle_attitude_setpoint
- msg: vehicle_attitude_groundtruth
id: 128
alias: vehicle_attitude
- msg: vehicle_global_position_groundtruth
id: 129
alias: vehicle_global_position
- msg: vehicle_local_position_groundtruth
id: 130
alias: vehicle_local_position
- msg: vehicle_mocap_odometry
alias: vehicle_odometry
id: 131
receive: true
- msg: vehicle_visual_odometry
id: 132
- msg: mc_virtual_rates_setpoint
id: 133
- msg: fw_virtual_rates_setpoint
id: 134
alias: vehicle_odometry
receive: true
- msg: vehicle_trajectory_waypoint_desired
id: 135
id: 133
alias: vehicle_trajectory_waypoint

2
src/drivers/gps/devices

@ -1 +1 @@
Subproject commit 69e38ecc846df03f0bb71aa136bcc830dd3316a9
Subproject commit a4999f111d13bcb209754823a3c503fa659a0d15

2
src/lib/ecl

@ -1 +1 @@
Subproject commit e1751188fd15b799cbfae86bd7373bb91206069b
Subproject commit 54ac147ae8930f1c280a0bd5c25fa90eaf4c6e0d

36
src/modules/micrortps_bridge/CMakeLists.txt
View File

@ -41,25 +41,37 @@ if(NOT FASTRTPSGEN)
endif()
if (EXISTS "${PX4_SOURCE_DIR}/msg/tools/uorb_rtps_message_ids.yaml")
set(config_rtps_send_topics)
set(config_rtps_send_topics)
execute_process(
COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/msg/tools/uorb_rtps_classifier.py -s
COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/msg/tools/uorb_rtps_classifier.py -sa
OUTPUT_VARIABLE config_rtps_send_topics
)
set(config_rtps_send_alias_topics "")
string(FIND ${config_rtps_send_topics} "alias" found_send_alias)
if (NOT ${found_send_alias} EQUAL "-1")
STRING(REGEX REPLACE ".*alias " "" config_rtps_send_alias_topics "${config_rtps_send_topics}")
STRING(REPLACE ", " ";" config_rtps_send_alias_topics "${config_rtps_send_alias_topics}")
STRING(REPLACE "\n" "" config_rtps_send_alias_topics "${config_rtps_send_alias_topics}")
STRING(REGEX REPLACE " alias.*" "" config_rtps_send_topics "${config_rtps_send_topics}")
endif()
STRING(REGEX REPLACE ", " ";" config_rtps_send_topics "${config_rtps_send_topics}")
STRING(REGEX REPLACE "\n" "" config_rtps_send_topics "${config_rtps_send_topics}")
set(config_rtps_receive_topics)
execute_process(
COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/msg/tools/uorb_rtps_classifier.py -r
COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/msg/tools/uorb_rtps_classifier.py -ra
OUTPUT_VARIABLE config_rtps_receive_topics
)
STRING(REGEX REPLACE ", " ";" config_rtps_receive_topics "${config_rtps_receive_topics}")
STRING(REGEX REPLACE "\n" "" config_rtps_receive_topics "${config_rtps_receive_topics}")
set(config_rtps_receive_alias_topics "")
string(FIND ${config_rtps_receive_topics} "alias" found_receive_alias)
if (NOT ${found_receive_alias} EQUAL "-1")
STRING(REGEX REPLACE ".*alias " "" config_rtps_receive_alias_topics "${config_rtps_receive_topics}")
STRING(REPLACE ", " ";" config_rtps_receive_alias_topics "${config_rtps_receive_alias_topics}")
STRING(REPLACE "\n" "" config_rtps_receive_alias_topics "${config_rtps_receive_alias_topics}")
STRING(REGEX REPLACE " alias.*" "" config_rtps_receive_topics "${config_rtps_receive_topics}")
endif()
STRING(REPLACE ", " ";" config_rtps_receive_topics "${config_rtps_receive_topics}")
STRING(REPLACE "\n" "" config_rtps_receive_topics "${config_rtps_receive_topics}")
endif()
if (FASTRTPSGEN AND (config_rtps_send_topics OR config_rtps_receive_topics))
@ -87,7 +99,8 @@ if (GENERATE_RTPS_BRIDGE)
set(uorb_sources_microcdr)
# send topic files
message(STATUS "RTPS send: ${config_rtps_send_topics}")
STRING(REGEX REPLACE ";" ", " send_list "${config_rtps_send_topics};${config_rtps_send_alias_topics}")
message(STATUS "RTPS send: ${send_list}")
set(send_topic_files)
foreach(topic ${config_rtps_send_topics})
list(APPEND send_topic_files ${PX4_SOURCE_DIR}/msg/${topic}.msg)
@ -96,7 +109,8 @@ if (GENERATE_RTPS_BRIDGE)
endforeach()
# receive topic files
message(STATUS "RTPS receive: ${config_rtps_receive_topics}")
STRING(REGEX REPLACE ";" ", " rcv_list "${config_rtps_receive_topics};${config_rtps_receive_alias_topics}")
message(STATUS "RTPS receive: ${rcv_list}")
set(receive_topic_files)
foreach(topic ${config_rtps_receive_topics})
list(APPEND receive_topic_files ${PX4_SOURCE_DIR}/msg/${topic}.msg)

18
src/modules/micrortps_bridge/micrortps_client/CMakeLists.txt
View File

@ -45,6 +45,14 @@ if (NOT "${config_rtps_send_topics}" STREQUAL "" OR NOT "${config_rtps_receive_t
list(APPEND receive_topic_files ${PX4_SOURCE_DIR}/msg/${topic}.msg)
endforeach()
if (NOT "${config_rtps_send_alias_topics}" STREQUAL "")
set(config_rtps_send_topics "${config_rtps_send_topics};${config_rtps_send_alias_topics}")
endif()
if (NOT "${config_rtps_receive_alias_topics}" STREQUAL "")
set(config_rtps_receive_topics "${config_rtps_receive_topics};${config_rtps_receive_alias_topics}")
endif()
foreach(topic ${config_rtps_send_topics})
list(APPEND topic_bridge_files_out ${msg_out_path}/micrortps_agent/${topic}_Publisher.cpp)
list(APPEND topic_bridge_files_out ${msg_out_path}/micrortps_agent/${topic}_Publisher.h)
@ -55,16 +63,6 @@ if (NOT "${config_rtps_send_topics}" STREQUAL "" OR NOT "${config_rtps_receive_t
list(APPEND topic_bridge_files_out ${msg_out_path}/micrortps_agent/${topic}_Subscriber.h)
endforeach()
set(send_topic_files_opt)
if (NOT "${send_topic_files}" STREQUAL "")
set(send_topic_opt "--send")
endif()
set(receive_topic_files_opt)
if (NOT "${receive_topic_files}" STREQUAL "")
set(receive_topic_opt "--receive")
endif()
list(APPEND topic_bridge_files_out
${msg_out_path}/micrortps_client/microRTPS_client.cpp
${msg_out_path}/micrortps_client/microRTPS_transport.cpp