use a yaml file description instead of json; minor improvements and cleanup

This commit is contained in:
TSC21 2018-09-04 21:49:00 +01:00 committed by Beat Küng
parent 7d40c4f97e
commit bb835382dd
9 changed files with 239 additions and 374 deletions

View File

@ -10,16 +10,24 @@
@# - multi_topics (List) list of all multi-topic names
@###############################################
@{
import os
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 uorb_rtps_message_ids 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]
yaml_file = os.path.abspath('uorb_rtps_message_ids.yaml')
try:
msg_ids = parse_yaml_msg_id_file(yaml_file)
except OSError as e:
if e.errno == errno.ENOENT:
raise IOError(errno.ENOENT, os.strerror(errno.ENOENT), yaml_file)
else:
raise
}@
/****************************************************************************
*
@ -112,7 +120,7 @@ void* send(void* /*unused*/)
/* payload is shifted by header length to make room for header*/
serialize_@(topic)(&MicroBufferWriter, &data, &data_buffer[header_length], &length);
if (0 < (read = transport_node->write((char)@(message_id(topic)), data_buffer, length)))
if (0 < (read = transport_node->write((char)@(rtps_message_id(msg_ids, topic)), data_buffer, length)))
{
total_sent += read;
++sent;
@ -187,7 +195,8 @@ void micrortps_start_topics(struct timespec &begin, int &total_read, uint32_t &r
switch (topic_ID)
{
@[for topic in recv_topics]@
case @(message_id(topic)):
case @(rtps_message_id(msg_ids, topic)):
{
deserialize_@(topic)(&MicroBufferReader, &@(topic)_data, data_buffer);
if (!@(topic)_pub) {

View File

@ -19,6 +19,14 @@ 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]
yaml_file = os.path.abspath('uorb_rtps_message_ids.yaml')
try:
msg_ids = parse_yaml_msg_id_file(yaml_file)
except OSError as e:
if e.errno == errno.ENOENT:
raise IOError(errno.ENOENT, os.strerror(errno.ENOENT), yaml_file)
else:
raise
}@
/****************************************************************************
*
@ -89,7 +97,7 @@ void RtpsTopics::publish(uint8_t topic_ID, char data_buffer[], size_t len)
switch (topic_ID)
{
@[for topic in send_topics]@
case @(rtps_message_id(os.path.abspath('uorb_rtps_message_ids.json'), topic)): // @(topic)
case @(rtps_message_id(msg_ids, topic)): // @(topic)
{
@(topic)_ st;
eprosima::fastcdr::FastBuffer cdrbuffer(data_buffer, len);
@ -117,7 +125,7 @@ bool RtpsTopics::hasMsg(uint8_t *topic_ID)
switch (_sub_topics[_next_sub_idx])
{
@[for topic in recv_topics]@
case @(rtps_message_id(os.path.abspath('uorb_rtps_message_ids.json'), topic)): if (_@(topic)_sub.hasMsg()) *topic_ID = @(rtps_message_id(os.path.abspath('uorb_rtps_message_ids.json'), topic)); break;
case @(rtps_message_id(msg_ids, topic)): if (_@(topic)_sub.hasMsg()) *topic_ID = @(rtps_message_id(msg_ids, topic)); break;
@[end for]@
default:
printf("Unexpected topic ID to check hasMsg\n");
@ -141,7 +149,7 @@ bool RtpsTopics::getMsg(const uint8_t topic_ID, eprosima::fastcdr::Cdr &scdr)
switch (topic_ID)
{
@[for topic in recv_topics]@
case @(rtps_message_id(os.path.abspath('uorb_rtps_message_ids.json'), topic)): // @(topic)
case @(rtps_message_id(msg_ids, topic)): // @(topic)
if (_@(topic)_sub.hasMsg())
{
@(topic)_ msg = _@(topic)_sub.getMsg();

View File

@ -19,6 +19,14 @@ 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]
yaml_file = os.path.abspath('uorb_rtps_message_ids.yaml')
try:
msg_ids = parse_yaml_msg_id_file(yaml_file)
except OSError as e:
if e.errno == errno.ENOENT:
raise IOError(errno.ENOENT, os.strerror(errno.ENOENT), yaml_file)
else:
raise
}@
/****************************************************************************
*
@ -89,7 +97,7 @@ private:
unsigned _next_sub_idx = 0;
char _sub_topics[@(len(recv_topics))] = {
@[for topic in recv_topics]@
@(rtps_message_id(os.path.abspath('uorb_rtps_message_ids.json'), topic)), // @(topic)
@(rtps_message_id(msg_ids, topic)), // @(topic)
@[end for]@
};
@[end if]@

View File

@ -69,8 +69,8 @@ parser.add_argument("-a", "--agent", dest='agent', action="store_true", help="Fl
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("-i", "--no-idl", dest='idl', action="store_false", help="Flag for generate idl files for each msg, by default is true if -i 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("-b", "--uorb-templates-dir", dest='uorb_templates', type=str, nargs=1, help="uORB templates dir, by default msg_dir/templates/uorb_microcdr", default=default_uorb_templates_dir)
parser.add_argument("-q", "--urtps-templates-dir", dest='urtps_templates', type=str, nargs=1, help="uRTPS templates dir, by default msg_dir/templates/urtps", default=default_urtps_templates_dir)
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)
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)
parser.add_argument("-p", "--package", dest='package', type=str, nargs=1, help="Msg package naming, by default px4", default=default_package_name)
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=default_agent_out)
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=default_client_out)
@ -188,6 +188,7 @@ def generate_agent(out_dir):
# Final steps to install agent
mkdir_p(agent_out_dir + "/fastrtpsgen")
prev_cwd_path = os.getcwd()
os.chdir(agent_out_dir + "/fastrtpsgen")
for idl_file in glob.glob(agent_out_dir + "/idl/*.idl"):
ret = subprocess.call(fastrtpsgen_path + " -d " + agent_out_dir + "/fastrtpsgen -example x64Linux2.6gcc " + idl_file, shell=True)
@ -205,6 +206,7 @@ def generate_agent(out_dir):
cp_wildcard(urtps_templates_dir + "/microRTPS_transport.*", agent_out_dir)
os.rename(agent_out_dir + "/microRTPS_agent_CMakeLists.txt", agent_out_dir + "/CMakeLists.txt")
mkdir_p(agent_out_dir + "/build")
os.chdir(prev_cwd_path)
return 0
def rm_wildcard(pattern):

View File

@ -43,6 +43,7 @@ import shutil
import filecmp
import argparse
import sys
import errno
px4_tools_dir = os.path.dirname(os.path.abspath(__file__))
sys.path.append(px4_tools_dir + "/genmsg/src")
@ -271,11 +272,12 @@ def generate_by_template(output_file, template_file, em_globals):
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})
if not os.path.isfile(template_file):
ofile.close()
os.remove(output_file)
raise RuntimeError("Template file %s not found" % (template_file))
interpreter.file(open(template_file)) #todo try
try:
interpreter.file(open(template_file))
except OSError as e:
ofile.close()
os.remove(output_file)
raise
interpreter.shutdown()
ofile.close()
return True

View File

@ -4,7 +4,8 @@
# Another positive effect of having the code here, is that this file will get
# precompiled and thus message generation will be much faster
import json
import os
import yaml
import genmsg.msgs
import gencpp
@ -298,13 +299,23 @@ def print_field_def(field):
print('\t%s%s%s %s%s;%s'%(type_prefix, type_px4, type_appendix, field.name,
array_size, comment))
def rtps_message_id(json_file, message):
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 rtps_message_id(msg_id_map, message):
"""
Get RTPS ID of uORB message
"""
with open(json_file, 'r') as f:
msg_id_map = json.load(f)
for dict in msg_id_map["rtps"]:
if message in dict["msg"]:
return dict["id"]

View File

@ -1,352 +0,0 @@
{
"rtps": [
{
"msg": "actuator_armed",
"id": 1
},
{
"msg": "actuator_controls",
"id": 2
},
{
"msg": "actuator_direct",
"id": 3
},
{
"msg": "actuator_outputs",
"id": 4
},
{
"msg": "adc_report",
"id": 5
},
{
"msg": "airspeed",
"id": 6
},
{
"msg": "att_pos_mocap",
"id": 7
},
{
"msg": "battery_status",
"id": 8
},
{
"msg": "camera_capture",
"id": 9
},
{
"msg": "camera_trigger",
"id": 10
},
{
"msg": "collision_report",
"id": 11
},
{
"msg": "commander_state",
"id": 12
},
{
"msg": "cpuload",
"id": 14
},
{
"msg": "debug_key_value",
"id": 15
},
{
"msg": "differential_pressure",
"id": 16
},
{
"msg": "distance_sensor",
"id": 17
},
{
"msg": "ekf2_innovations",
"id": 18
},
{
"msg": "ekf2_timestamps",
"id": 20
},
{
"msg": "esc_report",
"id": 21
},
{
"msg": "esc_status",
"id": 22
},
{
"msg": "estimator_status",
"id": 23
},
{
"msg": "fence",
"id": 24
},
{
"msg": "fence_vertex",
"id": 25
},
{
"msg": "follow_target",
"id": 27
},
{
"msg": "fw_pos_ctrl_status",
"id": 28
},
{
"msg": "geofence_result",
"id": 29
},
{
"msg": "gps_dump",
"id": 30
},
{
"msg": "gps_inject_data",
"id": 31
},
{
"msg": "home_position",
"id": 33
},
{
"msg": "input_rc",
"id": 34
},
{
"msg": "led_control",
"id": 35
},
{
"msg": "log_message",
"id": 36
},
{
"msg": "manual_control_setpoint",
"id": 37
},
{
"msg": "mavlink_log",
"id": 38
},
{
"msg": "mc_att_ctrl_status",
"id": 39
},
{
"msg": "mission",
"id": 40
},
{
"msg": "mission_result",
"id": 41
},
{
"msg": "mount_orientation",
"id": 42
},
{
"msg": "multirotor_motor_limits",
"id": 43
},
{
"msg": "offboard_control_mode",
"id": 44
},
{
"msg": "optical_flow",
"id": 45
},
{
"msg": "parameter_update",
"id": 47
},
{
"msg": "position_setpoint",
"id": 48
},
{
"msg": "position_setpoint_triplet",
"id": 49
},
{
"msg": "pwm_input",
"id": 40
},
{
"msg": "qshell_req",
"id": 51
},
{
"msg": "rc_channels",
"id": 52
},
{
"msg": "rc_parameter_map",
"id": 53
},
{
"msg": "safety",
"id": 54
},
{
"msg": "satellite_info",
"id": 55
},
{
"msg": "sensor_accel",
"id": 56
},
{
"msg": "sensor_baro",
"id": 57
},
{
"msg": "sensor_combined",
"id": 58
},
{
"msg": "sensor_correction",
"id": 59
},
{
"msg": "sensor_gyro",
"id": 60
},
{
"msg": "sensor_mag",
"id": 61
},
{
"msg": "sensor_preflight",
"id": 62
},
{
"msg": "sensor_selection",
"id": 63
},
{
"msg": "servorail_status",
"id": 64
},
{
"msg": "subsystem_info",
"id": 65
},
{
"msg": "system_power",
"id": 66
},
{
"msg": "task_stack_info",
"id": 67
},
{
"msg": "tecs_status",
"id": 68
},
{
"msg": "telemetry_status",
"id": 69
},
{
"msg": "test_motor",
"id": 70
},
{
"msg": "time_offset",
"id": 71
},
{
"msg": "transponder_report",
"id": 72
},
{
"msg": "uavcan_parameter_request",
"id": 73
},
{
"msg": "uavcan_parameter_value",
"id": 74
},
{
"msg": "ulog_stream_ack",
"id": 75
},
{
"msg": "ulog_stream",
"id": 76
},
{
"msg": "vehicle_attitude",
"id": 77
},
{
"msg": "vehicle_attitude_setpoint",
"id": 78
},
{
"msg": "vehicle_command_ack",
"id": 79
},
{
"msg": "vehicle_command",
"id": 80
},
{
"msg": "vehicle_control_mode",
"id": 81
},
{
"msg": "vehicle_global_position",
"id": 83
},
{
"msg": "vehicle_gps_position",
"id": 85
},
{
"msg": "vehicle_land_detected",
"id": 86
},
{
"msg": "vehicle_local_position",
"id": 87
},
{
"msg": "vehicle_local_position_setpoint",
"id": 88
},
{
"msg": "vehicle_rates_setpoint",
"id": 89
},
{
"msg": "vehicle_roi",
"id": 90
},
{
"msg": "vehicle_status_flags",
"id": 91
},
{
"msg": "vehicle_status",
"id": 92
},
{
"msg": "vtol_vehicle_status",
"id": 93
},
{
"msg": "wind_estimate",
"id": 94
}
]
}

View File

@ -0,0 +1,175 @@
rtps:
- msg: actuator_armed
id: 1
- msg: actuator_controls
id: 2
- msg: actuator_direct
id: 3
- msg: actuator_outputs
id: 4
- msg: adc_report
id: 5
- msg: airspeed
id: 6
- msg: att_pos_mocap
id: 7
- msg: battery_status
id: 8
- msg: camera_capture
id: 9
- msg: camera_trigger
id: 10
- msg: collision_report
id: 11
- msg: commander_state
id: 12
- msg: cpuload
id: 14
- msg: debug_key_value
id: 15
- msg: differential_pressure
id: 16
- msg: distance_sensor
id: 17
- msg: ekf2_innovations
id: 18
- msg: ekf2_timestamps
id: 20
- msg: esc_report
id: 21
- msg: esc_status
id: 22
- msg: estimator_status
id: 23
- msg: fence
id: 24
- msg: fence_vertex
id: 25
- msg: follow_target
id: 27
- msg: fw_pos_ctrl_status
id: 28
- msg: geofence_result
id: 29
- msg: gps_dump
id: 30
- msg: gps_inject_data
id: 31
- msg: home_position
id: 33
- msg: input_rc
id: 34
- msg: led_control
id: 35
- msg: log_message
id: 36
- msg: manual_control_setpoint
id: 37
- msg: mavlink_log
id: 38
- msg: mc_att_ctrl_status
id: 39
- msg: mission
id: 40
- msg: mission_result
id: 41
- msg: mount_orientation
id: 42
- msg: multirotor_motor_limits
id: 43
- msg: offboard_control_mode
id: 44
- msg: optical_flow
id: 45
- msg: parameter_update
id: 47
- msg: position_setpoint
id: 48
- msg: position_setpoint_triplet
id: 49
- msg: pwm_input
id: 40
- msg: qshell_req
id: 51
- msg: rc_channels
id: 52
- msg: rc_parameter_map
id: 53
- msg: safety
id: 54
- msg: satellite_info
id: 55
- msg: sensor_accel
id: 56
- msg: sensor_baro
id: 57
- msg: sensor_combined
id: 58
- msg: sensor_correction
id: 59
- msg: sensor_gyro
id: 60
- msg: sensor_mag
id: 61
- msg: sensor_preflight
id: 62
- msg: sensor_selection
id: 63
- msg: servorail_status
id: 64
- msg: subsystem_info
id: 65
- msg: system_power
id: 66
- msg: task_stack_info
id: 67
- msg: tecs_status
id: 68
- msg: telemetry_status
id: 69
- msg: test_motor
id: 70
- msg: time_offset
id: 71
- msg: transponder_report
id: 72
- msg: uavcan_parameter_request
id: 73
- msg: uavcan_parameter_value
id: 74
- msg: ulog_stream_ack
id: 75
- msg: ulog_stream
id: 76
- msg: vehicle_attitude
id: 77
- msg: vehicle_attitude_setpoint
id: 78
- msg: vehicle_command_ack
id: 79
- msg: vehicle_command
id: 80
- msg: vehicle_control_mode
id: 81
- msg: vehicle_global_position
id: 83
- msg: vehicle_gps_position
id: 85
- msg: vehicle_land_detected
id: 86
- msg: vehicle_local_position
id: 87
- msg: vehicle_local_position_setpoint
id: 88
- msg: vehicle_rates_setpoint
id: 89
- msg: vehicle_roi
id: 90
- msg: vehicle_status_flags
id: 91
- msg: vehicle_status
id: 92
- msg: vtol_vehicle_status
id: 93
- msg: wind_estimate
id: 94

View File

@ -72,8 +72,8 @@ if (NOT "${config_rtps_send_topics}" STREQUAL "" OR NOT "${config_rtps_receive_t
)
# Copy json file to the bin dir to be used by empy on the file generation
configure_file("${PX4_SOURCE_DIR}/msg/tools/uorb_rtps_message_ids.json"
"${CMAKE_CURRENT_BINARY_DIR}/uorb_rtps_message_ids.json" COPYONLY)
configure_file("${PX4_SOURCE_DIR}/msg/tools/uorb_rtps_message_ids.yaml"
"${CMAKE_CURRENT_BINARY_DIR}/uorb_rtps_message_ids.yaml" COPYONLY)
add_custom_command(OUTPUT ${topic_bridge_files_out}
COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/msg/tools/generate_microRTPS_bridge.py
@ -81,6 +81,8 @@ if (NOT "${config_rtps_send_topics}" STREQUAL "" OR NOT "${config_rtps_receive_t
${send_topic_opt} ${send_topic_files}
${receive_topic_opt} ${receive_topic_files}
--topic-msg-dir ${PX4_SOURCE_DIR}/msg
--uorb-templates-dir "/templates/uorb_microcdr"
--urtps-templates-dir "/templates/urtps"
--agent-outdir ${CMAKE_CURRENT_BINARY_DIR}/micrortps_agent
--client-outdir ${CMAKE_CURRENT_BINARY_DIR}/micrortps_client
>micrortps_bridge.log 2>&1 || cat micrortps_bridge.log # quiet successful build output