From bb835382dd4b02534163d6fc787528f4d22fd58f Mon Sep 17 00:00:00 2001 From: TSC21 Date: Tue, 4 Sep 2018 21:49:00 +0100 Subject: [PATCH] use a yaml file description instead of json; minor improvements and cleanup --- .../microRTPS_client.cpp.template | 17 +- msg/templates/urtps/RtpsTopics.cpp.template | 14 +- msg/templates/urtps/RtpsTopics.h.template | 10 +- msg/tools/generate_microRTPS_bridge.py | 6 +- msg/tools/px_generate_uorb_topic_files.py | 12 +- msg/tools/px_generate_uorb_topic_helper.py | 21 +- msg/tools/uorb_rtps_message_ids.json | 352 ------------------ msg/tools/uorb_rtps_message_ids.yaml | 175 +++++++++ .../micrortps_client/CMakeLists.txt | 6 +- 9 files changed, 239 insertions(+), 374 deletions(-) delete mode 100644 msg/tools/uorb_rtps_message_ids.json create mode 100644 msg/tools/uorb_rtps_message_ids.yaml diff --git a/msg/templates/uorb_microcdr/microRTPS_client.cpp.template b/msg/templates/uorb_microcdr/microRTPS_client.cpp.template index f2e98543c0..a722fb7354 100644 --- a/msg/templates/uorb_microcdr/microRTPS_client.cpp.template +++ b/msg/templates/uorb_microcdr/microRTPS_client.cpp.template @@ -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) { diff --git a/msg/templates/urtps/RtpsTopics.cpp.template b/msg/templates/urtps/RtpsTopics.cpp.template index b787fc3b7f..d08c64dcb7 100644 --- a/msg/templates/urtps/RtpsTopics.cpp.template +++ b/msg/templates/urtps/RtpsTopics.cpp.template @@ -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(); diff --git a/msg/templates/urtps/RtpsTopics.h.template b/msg/templates/urtps/RtpsTopics.h.template index 9dda842f62..b92dd36575 100644 --- a/msg/templates/urtps/RtpsTopics.h.template +++ b/msg/templates/urtps/RtpsTopics.h.template @@ -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]@ diff --git a/msg/tools/generate_microRTPS_bridge.py b/msg/tools/generate_microRTPS_bridge.py index f90e135601..0ed05eca9f 100644 --- a/msg/tools/generate_microRTPS_bridge.py +++ b/msg/tools/generate_microRTPS_bridge.py @@ -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): diff --git a/msg/tools/px_generate_uorb_topic_files.py b/msg/tools/px_generate_uorb_topic_files.py index 2081d84f6e..2a877ff33a 100755 --- a/msg/tools/px_generate_uorb_topic_files.py +++ b/msg/tools/px_generate_uorb_topic_files.py @@ -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 diff --git a/msg/tools/px_generate_uorb_topic_helper.py b/msg/tools/px_generate_uorb_topic_helper.py index 21a9f1091f..b13a53195a 100644 --- a/msg/tools/px_generate_uorb_topic_helper.py +++ b/msg/tools/px_generate_uorb_topic_helper.py @@ -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"] diff --git a/msg/tools/uorb_rtps_message_ids.json b/msg/tools/uorb_rtps_message_ids.json deleted file mode 100644 index db5308f9f8..0000000000 --- a/msg/tools/uorb_rtps_message_ids.json +++ /dev/null @@ -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 - } - ] -} diff --git a/msg/tools/uorb_rtps_message_ids.yaml b/msg/tools/uorb_rtps_message_ids.yaml new file mode 100644 index 0000000000..a6b617e999 --- /dev/null +++ b/msg/tools/uorb_rtps_message_ids.yaml @@ -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 diff --git a/src/modules/micrortps_bridge/micrortps_client/CMakeLists.txt b/src/modules/micrortps_bridge/micrortps_client/CMakeLists.txt index fa0c6630d9..0cb7a0433c 100644 --- a/src/modules/micrortps_bridge/micrortps_client/CMakeLists.txt +++ b/src/modules/micrortps_bridge/micrortps_client/CMakeLists.txt @@ -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