initial version of msg to uorb script

Standard and embedded types work, may need small refinements for some
types
This commit is contained in:
Thomas Gubler 2014-12-01 16:39:27 +01:00
parent 6b695ac9e8
commit 8b5bc703a1
39 changed files with 415 additions and 13 deletions

1
.gitignore vendored
View File

@ -39,3 +39,4 @@ tags
.pydevproject
.ropeproject
*.orig
src/modules/uORB/topics/*

View File

@ -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

View File

@ -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)

View File

@ -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

View File

@ -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

View File

@ -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

1
msg/std_msgs/Bool.msg Normal file
View File

@ -0,0 +1 @@
bool data

1
msg/std_msgs/Byte.msg Normal file
View File

@ -0,0 +1 @@
byte data

View File

@ -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

1
msg/std_msgs/Char.msg Normal file
View File

@ -0,0 +1 @@
char data

View File

@ -0,0 +1,4 @@
float32 r
float32 g
float32 b
float32 a

View File

@ -0,0 +1 @@
duration data

0
msg/std_msgs/Empty.msg Normal file
View File

1
msg/std_msgs/Float32.msg Normal file
View File

@ -0,0 +1 @@
float32 data

View File

@ -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

1
msg/std_msgs/Float64.msg Normal file
View File

@ -0,0 +1 @@
float64 data

View File

@ -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

15
msg/std_msgs/Header.msg Normal file
View File

@ -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

1
msg/std_msgs/Int16.msg Normal file
View File

@ -0,0 +1 @@
int16 data

View File

@ -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

1
msg/std_msgs/Int32.msg Normal file
View File

@ -0,0 +1 @@
int32 data

View File

@ -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

1
msg/std_msgs/Int64.msg Normal file
View File

@ -0,0 +1 @@
int64 data

View File

@ -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

1
msg/std_msgs/Int8.msg Normal file
View File

@ -0,0 +1 @@
int8 data

View File

@ -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

View File

@ -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

View File

@ -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.

1
msg/std_msgs/String.msg Normal file
View File

@ -0,0 +1 @@
string data

1
msg/std_msgs/Time.msg Normal file
View File

@ -0,0 +1 @@
time data

1
msg/std_msgs/UInt16.msg Normal file
View File

@ -0,0 +1 @@
uint16 data

View File

@ -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

1
msg/std_msgs/UInt32.msg Normal file
View File

@ -0,0 +1 @@
uint32 data

View File

@ -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

1
msg/std_msgs/UInt64.msg Normal file
View File

@ -0,0 +1 @@
uint64 data

View File

@ -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

1
msg/std_msgs/UInt8.msg Normal file
View File

@ -0,0 +1 @@
uint8 data

View File

@ -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

View File

@ -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));