forked from Archive/PX4-Autopilot
initial version of msg to uorb script
Standard and embedded types work, may need small refinements for some types
This commit is contained in:
parent
6b695ac9e8
commit
8b5bc703a1
|
@ -39,3 +39,4 @@ tags
|
|||
.pydevproject
|
||||
.ropeproject
|
||||
*.orig
|
||||
src/modules/uORB/topics/*
|
||||
|
|
20
Makefile
20
Makefile
|
@ -104,13 +104,13 @@ DESIRED_FIRMWARES = $(foreach config,$(CONFIGS),$(IMAGE_DIR)$(config).px4)
|
|||
STAGED_FIRMWARES = $(foreach config,$(KNOWN_CONFIGS),$(IMAGE_DIR)$(config).px4)
|
||||
FIRMWARES = $(foreach config,$(KNOWN_CONFIGS),$(BUILD_DIR)$(config).build/firmware.px4)
|
||||
|
||||
all: checksubmodules $(DESIRED_FIRMWARES)
|
||||
all: checksubmodules generateuorbtopicheaders $(DESIRED_FIRMWARES)
|
||||
|
||||
#
|
||||
# Copy FIRMWARES into the image directory.
|
||||
#
|
||||
# XXX copying the .bin files is a hack to work around the PX4IO uploader
|
||||
# not supporting .px4 files, and it should be deprecated onced that
|
||||
# XXX copying the .bin files is a hack to work around the PX4IO uploader
|
||||
# not supporting .px4 files, and it should be deprecated onced that
|
||||
# is taken care of.
|
||||
#
|
||||
$(STAGED_FIRMWARES): $(IMAGE_DIR)%.px4: $(BUILD_DIR)%.build/firmware.px4
|
||||
|
@ -152,7 +152,7 @@ $(foreach config,$(FMU_CONFIGS),$(eval $(call FMU_DEP,$(config))))
|
|||
# Build the NuttX export archives.
|
||||
#
|
||||
# Note that there are no explicit dependencies extended from these
|
||||
# archives. If NuttX is updated, the user is expected to rebuild the
|
||||
# archives. If NuttX is updated, the user is expected to rebuild the
|
||||
# archives/build area manually. Likewise, when the 'archives' target is
|
||||
# invoked, all archives are always rebuilt.
|
||||
#
|
||||
|
@ -224,6 +224,16 @@ updatesubmodules:
|
|||
$(Q) (git submodule init)
|
||||
$(Q) (git submodule update)
|
||||
|
||||
MSG_DIR = $(PX4_BASE)msg/px4_msgs
|
||||
MSG_TEMPLATE_DIR = $(PX4_BASE)msg/templates
|
||||
TOPICS_DIR = $(PX4_BASE)src/modules/uORB/topics
|
||||
|
||||
.PHONY: generateuorbtopicheaders
|
||||
generateuorbtopicheaders:
|
||||
@$(ECHO) "Generating uORB topic headers"
|
||||
$(Q) ($(PX4_BASE)/Tools/px_generate_uorb_topic_headers.py -d $(MSG_DIR) \
|
||||
-o $(TOPICS_DIR) -e $(MSG_TEMPLATE_DIR))
|
||||
|
||||
#
|
||||
# Testing targets
|
||||
#
|
||||
|
@ -232,7 +242,7 @@ testbuild:
|
|||
|
||||
#
|
||||
# Cleanup targets. 'clean' should remove all built products and force
|
||||
# a complete re-compilation, 'distclean' should remove everything
|
||||
# a complete re-compilation, 'distclean' should remove everything
|
||||
# that's generated leaving only files that are in source control.
|
||||
#
|
||||
.PHONY: clean
|
||||
|
|
|
@ -0,0 +1,98 @@
|
|||
#!/usr/bin/env python
|
||||
#############################################################################
|
||||
#
|
||||
# Copyright (C) 2013-2014 PX4 Development Team. All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
# are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright
|
||||
# notice, this list of conditions and the following disclaimer.
|
||||
# 2. Redistributions in binary form must reproduce the above copyright
|
||||
# notice, this list of conditions and the following disclaimer in
|
||||
# the documentation and/or other materials provided with the
|
||||
# distribution.
|
||||
# 3. Neither the name PX4 nor the names of its contributors may be
|
||||
# used to endorse or promote products derived from this software
|
||||
# without specific prior written permission.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
#############################################################################
|
||||
|
||||
"""
|
||||
px_generate_uorb_topic_headers.py
|
||||
Generates c/cpp header files for uorb topics from .msg (ROS syntax)
|
||||
message files
|
||||
"""
|
||||
from __future__ import print_function
|
||||
import os
|
||||
import argparse
|
||||
import genmsg.template_tools
|
||||
|
||||
__author__ = "Thomas Gubler"
|
||||
__copyright__ = "Copyright (C) 2013-2014 PX4 Development Team."
|
||||
__license__ = "BSD"
|
||||
__email__ = "thomasgubler@gmail.com"
|
||||
|
||||
|
||||
msg_template_map = {'msg.h.template': '@NAME@.h'}
|
||||
srv_template_map = {}
|
||||
incl_default = ['std_msgs:./msg/std_msgs']
|
||||
package = 'px4'
|
||||
|
||||
|
||||
def convert_file(filename, outputdir, templatedir, includepath):
|
||||
"""
|
||||
Converts a single .msg file to a uorb header
|
||||
"""
|
||||
print("Generating uORB headers from {0}".format(filename))
|
||||
genmsg.template_tools.generate_from_file(filename,
|
||||
package,
|
||||
outputdir,
|
||||
templatedir,
|
||||
includepath,
|
||||
msg_template_map,
|
||||
srv_template_map)
|
||||
|
||||
|
||||
def convert_dir(inputdir, outputdir, templatedir):
|
||||
"""
|
||||
Converts all .msg files in inputdir to uORB header files
|
||||
"""
|
||||
includepath = incl_default + [':'.join([package, inputdir])]
|
||||
for f in os.listdir(inputdir):
|
||||
fn = os.path.join(inputdir, f)
|
||||
if os.path.isfile(fn):
|
||||
convert_file(fn, outputdir, templatedir, includepath)
|
||||
|
||||
if __name__ == "__main__":
|
||||
parser = argparse.ArgumentParser(
|
||||
description='Convert msg files to uorb headers')
|
||||
parser.add_argument('-d', dest='dir', help='directory with msg files')
|
||||
parser.add_argument('-f', dest='file',
|
||||
help="files to convert (use only without -d)",
|
||||
nargs="+")
|
||||
parser.add_argument('-e', dest='templatedir',
|
||||
help='directory with template files',)
|
||||
parser.add_argument('-o', dest='outputdir',
|
||||
help='output directory for header files')
|
||||
args = parser.parse_args()
|
||||
|
||||
if args.file is not None:
|
||||
for f in args.file:
|
||||
convert_file(f, args.outputdir, args.templatedir, incl_default)
|
||||
elif args.dir is not None:
|
||||
convert_dir(args.dir, args.outputdir, args.templatedir)
|
|
@ -0,0 +1,6 @@
|
|||
|
||||
uint64 timestamp # Microseconds since system boot
|
||||
bool armed # Set to true if system is armed
|
||||
bool ready_to_arm # Set to true if system is ready to be armed
|
||||
bool lockdown # Set to true if actuators are forced to being disabled (due to emergency or HIL)
|
||||
bool force_failsafe # Set to true if the actuators are forced to the failsafe position
|
|
@ -0,0 +1,24 @@
|
|||
int32 RC_CHANNELS_FUNCTION_MAX=18
|
||||
uint8 RC_CHANNELS_FUNCTION_THROTTLE=0
|
||||
uint8 RC_CHANNELS_FUNCTION_ROLL=1
|
||||
uint8 RC_CHANNELS_FUNCTION_PITCH=2
|
||||
uint8 RC_CHANNELS_FUNCTION_YAW=3
|
||||
uint8 RC_CHANNELS_FUNCTION_MODE=4
|
||||
uint8 RC_CHANNELS_FUNCTION_RETURN=5
|
||||
uint8 RC_CHANNELS_FUNCTION_POSCTL=6
|
||||
uint8 RC_CHANNELS_FUNCTION_LOITER=7
|
||||
uint8 RC_CHANNELS_FUNCTION_OFFBOARD=8
|
||||
uint8 RC_CHANNELS_FUNCTION_ACRO=9
|
||||
uint8 RC_CHANNELS_FUNCTION_FLAPS=10
|
||||
uint8 RC_CHANNELS_FUNCTION_AUX_1=11
|
||||
uint8 RC_CHANNELS_FUNCTION_AUX_2=12
|
||||
uint8 RC_CHANNELS_FUNCTION_AUX_3=13
|
||||
uint8 RC_CHANNELS_FUNCTION_AUX_4=14
|
||||
uint8 RC_CHANNELS_FUNCTION_AUX_5=15
|
||||
uint64 timestamp_last_valid # Timestamp of last valid RC signal
|
||||
float32[18] channels # Scaled to -1..1 (throttle: 0..1)
|
||||
uint8 channel_count # Number of valid channels
|
||||
int8[18] function # Functions mapping
|
||||
uint8 rssi # Receive signal strength index
|
||||
bool signal_lost # Control signal lost, should be checked together with topic timeout
|
||||
actuator_armed actuator
|
|
@ -1,8 +0,0 @@
|
|||
Header header
|
||||
int32 RC_CHANNELS_FUNCTION_MAX=18
|
||||
uint64 timestamp_last_valid # Timestamp of last valid RC signal
|
||||
float32[18] channels # Scaled to -1..1 (throttle: 0..1)
|
||||
uint8 channel_count # Number of valid channels
|
||||
int8[18] function # Functions mapping
|
||||
uint8 rssi # Receive signal strength index
|
||||
bool signal_lost # Control signal lost, should be checked together with topic timeout
|
|
@ -0,0 +1 @@
|
|||
bool data
|
|
@ -0,0 +1 @@
|
|||
byte data
|
|
@ -0,0 +1,6 @@
|
|||
# Please look at the MultiArrayLayout message definition for
|
||||
# documentation on all multiarrays.
|
||||
|
||||
MultiArrayLayout layout # specification of data layout
|
||||
byte[] data # array of data
|
||||
|
|
@ -0,0 +1 @@
|
|||
char data
|
|
@ -0,0 +1,4 @@
|
|||
float32 r
|
||||
float32 g
|
||||
float32 b
|
||||
float32 a
|
|
@ -0,0 +1 @@
|
|||
duration data
|
|
@ -0,0 +1 @@
|
|||
float32 data
|
|
@ -0,0 +1,6 @@
|
|||
# Please look at the MultiArrayLayout message definition for
|
||||
# documentation on all multiarrays.
|
||||
|
||||
MultiArrayLayout layout # specification of data layout
|
||||
float32[] data # array of data
|
||||
|
|
@ -0,0 +1 @@
|
|||
float64 data
|
|
@ -0,0 +1,6 @@
|
|||
# Please look at the MultiArrayLayout message definition for
|
||||
# documentation on all multiarrays.
|
||||
|
||||
MultiArrayLayout layout # specification of data layout
|
||||
float64[] data # array of data
|
||||
|
|
@ -0,0 +1,15 @@
|
|||
# Standard metadata for higher-level stamped data types.
|
||||
# This is generally used to communicate timestamped data
|
||||
# in a particular coordinate frame.
|
||||
#
|
||||
# sequence ID: consecutively increasing ID
|
||||
uint32 seq
|
||||
#Two-integer timestamp that is expressed as:
|
||||
# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')
|
||||
# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')
|
||||
# time-handling sugar is provided by the client library
|
||||
time stamp
|
||||
#Frame this data is associated with
|
||||
# 0: no frame
|
||||
# 1: global frame
|
||||
string frame_id
|
|
@ -0,0 +1 @@
|
|||
int16 data
|
|
@ -0,0 +1,6 @@
|
|||
# Please look at the MultiArrayLayout message definition for
|
||||
# documentation on all multiarrays.
|
||||
|
||||
MultiArrayLayout layout # specification of data layout
|
||||
int16[] data # array of data
|
||||
|
|
@ -0,0 +1 @@
|
|||
int32 data
|
|
@ -0,0 +1,6 @@
|
|||
# Please look at the MultiArrayLayout message definition for
|
||||
# documentation on all multiarrays.
|
||||
|
||||
MultiArrayLayout layout # specification of data layout
|
||||
int32[] data # array of data
|
||||
|
|
@ -0,0 +1 @@
|
|||
int64 data
|
|
@ -0,0 +1,6 @@
|
|||
# Please look at the MultiArrayLayout message definition for
|
||||
# documentation on all multiarrays.
|
||||
|
||||
MultiArrayLayout layout # specification of data layout
|
||||
int64[] data # array of data
|
||||
|
|
@ -0,0 +1 @@
|
|||
int8 data
|
|
@ -0,0 +1,6 @@
|
|||
# Please look at the MultiArrayLayout message definition for
|
||||
# documentation on all multiarrays.
|
||||
|
||||
MultiArrayLayout layout # specification of data layout
|
||||
int8[] data # array of data
|
||||
|
|
@ -0,0 +1,3 @@
|
|||
string label # label of given dimension
|
||||
uint32 size # size of given dimension (in type units)
|
||||
uint32 stride # stride of given dimension
|
|
@ -0,0 +1,26 @@
|
|||
# The multiarray declares a generic multi-dimensional array of a
|
||||
# particular data type. Dimensions are ordered from outer most
|
||||
# to inner most.
|
||||
|
||||
MultiArrayDimension[] dim # Array of dimension properties
|
||||
uint32 data_offset # padding bytes at front of data
|
||||
|
||||
# Accessors should ALWAYS be written in terms of dimension stride
|
||||
# and specified outer-most dimension first.
|
||||
#
|
||||
# multiarray(i,j,k) = data[data_offset + dim_stride[1]*i + dim_stride[2]*j + k]
|
||||
#
|
||||
# A standard, 3-channel 640x480 image with interleaved color channels
|
||||
# would be specified as:
|
||||
#
|
||||
# dim[0].label = "height"
|
||||
# dim[0].size = 480
|
||||
# dim[0].stride = 3*640*480 = 921600 (note dim[0] stride is just size of image)
|
||||
# dim[1].label = "width"
|
||||
# dim[1].size = 640
|
||||
# dim[1].stride = 3*640 = 1920
|
||||
# dim[2].label = "channel"
|
||||
# dim[2].size = 3
|
||||
# dim[2].stride = 3
|
||||
#
|
||||
# multiarray(i,j,k) refers to the ith row, jth column, and kth channel.
|
|
@ -0,0 +1 @@
|
|||
string data
|
|
@ -0,0 +1 @@
|
|||
time data
|
|
@ -0,0 +1 @@
|
|||
uint16 data
|
|
@ -0,0 +1,6 @@
|
|||
# Please look at the MultiArrayLayout message definition for
|
||||
# documentation on all multiarrays.
|
||||
|
||||
MultiArrayLayout layout # specification of data layout
|
||||
uint16[] data # array of data
|
||||
|
|
@ -0,0 +1 @@
|
|||
uint32 data
|
|
@ -0,0 +1,6 @@
|
|||
# Please look at the MultiArrayLayout message definition for
|
||||
# documentation on all multiarrays.
|
||||
|
||||
MultiArrayLayout layout # specification of data layout
|
||||
uint32[] data # array of data
|
||||
|
|
@ -0,0 +1 @@
|
|||
uint64 data
|
|
@ -0,0 +1,6 @@
|
|||
# Please look at the MultiArrayLayout message definition for
|
||||
# documentation on all multiarrays.
|
||||
|
||||
MultiArrayLayout layout # specification of data layout
|
||||
uint64[] data # array of data
|
||||
|
|
@ -0,0 +1 @@
|
|||
uint8 data
|
|
@ -0,0 +1,6 @@
|
|||
# Please look at the MultiArrayLayout message definition for
|
||||
# documentation on all multiarrays.
|
||||
|
||||
MultiArrayLayout layout # specification of data layout
|
||||
uint8[] data # array of data
|
||||
|
|
@ -0,0 +1,141 @@
|
|||
@###############################################
|
||||
@#
|
||||
@# PX4 ROS compatible message source code
|
||||
@# generation for C++
|
||||
@#
|
||||
@# EmPy template for generating <msg>.h files
|
||||
@# Based on the original template for ROS
|
||||
@#
|
||||
@###############################################
|
||||
@# Start of Template
|
||||
@#
|
||||
@# Context:
|
||||
@# - file_name_in (String) Source file
|
||||
@# - spec (msggen.MsgSpec) Parsed specification of the .msg file
|
||||
@# - md5sum (String) MD5Sum of the .msg specification
|
||||
@###############################################
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2013-2014 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/* Auto-generated by genmsg_cpp from file @file_name_in */
|
||||
|
||||
@{
|
||||
import genmsg.msgs
|
||||
import gencpp
|
||||
|
||||
cpp_namespace = '::%s::'%(spec.package) # TODO handle nested namespace
|
||||
cpp_class = '%s_'%spec.short_name
|
||||
cpp_full_name = '%s%s'%(cpp_namespace,cpp_class)
|
||||
cpp_full_name_with_alloc = '%s<ContainerAllocator>'%(cpp_full_name)
|
||||
cpp_msg_definition = gencpp.escape_message_definition(msg_definition)
|
||||
}@
|
||||
|
||||
#pragma once
|
||||
|
||||
@##############################
|
||||
@# Generic Includes
|
||||
@##############################
|
||||
#include <stdint.h>
|
||||
#include "../uORB.h"
|
||||
|
||||
@##############################
|
||||
@# Includes for dependencies
|
||||
@##############################
|
||||
@{
|
||||
for field in spec.parsed_fields():
|
||||
if (not field.is_builtin):
|
||||
if (not field.is_header):
|
||||
(package, name) = genmsg.names.package_resource_name(field.base_type)
|
||||
package = package or spec.package # convert '' to package
|
||||
print('#include <uORB/topics/%s.h>'%(name))
|
||||
|
||||
}@
|
||||
@# Constants
|
||||
@[for constant in spec.constants]@
|
||||
#define @(constant.name) = @(int(constant.val))
|
||||
@[end for]
|
||||
|
||||
/**
|
||||
* @@addtogroup topics
|
||||
* @@{
|
||||
*/
|
||||
|
||||
@##############################
|
||||
@# Main struct of message
|
||||
@##############################
|
||||
@{
|
||||
|
||||
inttypes = ['int8', 'int16', 'int32', 'int64', 'uint8', 'uint16', 'uint32', 'uint64']
|
||||
|
||||
# Function to print a standard ros type
|
||||
def print_field_def(field):
|
||||
type = field.type
|
||||
# detect embedded types
|
||||
sl_pos = type.find('/')
|
||||
type_appendix = ''
|
||||
type_prefix = ''
|
||||
if (sl_pos >= 0):
|
||||
type = type[sl_pos + 1:]
|
||||
type_prefix = 'struct '
|
||||
type_appendix = '_s'
|
||||
|
||||
# detect arrays
|
||||
a_pos = type.find('[')
|
||||
array_size = ''
|
||||
if (a_pos >= 0):
|
||||
# field is array
|
||||
array_size = type[a_pos:]
|
||||
type = type[:a_pos]
|
||||
|
||||
if type in inttypes:
|
||||
# need to add _t: int8 --> int8_t
|
||||
type_appendix = '_t'
|
||||
|
||||
print('\t%s%s%s%s %s;'%(type_prefix, type, type_appendix, array_size, field.name))
|
||||
|
||||
}
|
||||
struct @(spec.short_name)_s
|
||||
@{
|
||||
# loop over all fields and print the type and name
|
||||
for field in spec.parsed_fields():
|
||||
if (not field.is_header):
|
||||
print_field_def(field)
|
||||
}@
|
||||
}
|
||||
|
||||
/**
|
||||
* @@}
|
||||
*/
|
||||
|
||||
/* register this as object request broker structure */
|
||||
ORB_DECLARE(@(spec.short_name));
|
Loading…
Reference in New Issue