diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2014-12-04 13:36:16 +0100 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-12-04 13:36:16 +0100 |
commit | 0b9ef53ac123fa1a4e26ca933a7fab44248f45df (patch) | |
tree | c0d3f45d3d66f4b22cbf878a2282b2f38f063661 | |
parent | ad499a594447d807b3702c64fdfaad51ac579e2b (diff) | |
parent | 9b680ebeb6200d0229b9dcb0f223ceb3b14822d5 (diff) | |
download | px4-firmware-0b9ef53ac123fa1a4e26ca933a7fab44248f45df.tar.gz px4-firmware-0b9ef53ac123fa1a4e26ca933a7fab44248f45df.tar.bz2 px4-firmware-0b9ef53ac123fa1a4e26ca933a7fab44248f45df.zip |
Merge branch 'dev_ros' into dev_ros_rossharedlib
-rw-r--r-- | Makefile | 5 | ||||
-rwxr-xr-x | Tools/px_generate_uorb_topic_headers.py | 121 | ||||
-rw-r--r-- | makefiles/setup.mk | 6 | ||||
-rw-r--r-- | src/examples/subscriber/module.mk | 3 | ||||
-rw-r--r-- | src/examples/subscriber/subscriber.cpp | 15 | ||||
-rw-r--r-- | src/examples/subscriber/subscriber_params.c | 56 | ||||
-rw-r--r-- | src/examples/subscriber/subscriber_params.h | 43 | ||||
-rw-r--r-- | src/include/px4.h | 21 | ||||
-rw-r--r-- | src/platforms/px4_defines.h | 67 | ||||
-rw-r--r-- | src/platforms/px4_includes.h | 61 | ||||
-rw-r--r-- | src/platforms/px4_middleware.h | 13 | ||||
-rw-r--r-- | src/platforms/px4_nodehandle.h | 96 | ||||
-rw-r--r-- | src/platforms/px4_publisher.h | 35 | ||||
-rw-r--r-- | src/platforms/px4_subscriber.h | 41 |
14 files changed, 483 insertions, 100 deletions
@@ -227,12 +227,15 @@ updatesubmodules: MSG_DIR = $(PX4_BASE)msg/px4_msgs MSG_TEMPLATE_DIR = $(PX4_BASE)msg/templates TOPICS_DIR = $(PX4_BASE)src/modules/uORB/topics +TOPICS_TEMPORARY_DIR = $(BUILD_DIR)topics_temporary .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)) + -o $(TOPICS_DIR) -e $(MSG_TEMPLATE_DIR) -t $(TOPICS_TEMPORARY_DIR)) +# clean up temporary files + $(Q) (rm -r $(TOPICS_TEMPORARY_DIR)) # # Testing targets diff --git a/Tools/px_generate_uorb_topic_headers.py b/Tools/px_generate_uorb_topic_headers.py index a738dcb7e..2ddbd6984 100755 --- a/Tools/px_generate_uorb_topic_headers.py +++ b/Tools/px_generate_uorb_topic_headers.py @@ -39,6 +39,8 @@ message files """ from __future__ import print_function import os +import shutil +import filecmp import argparse import genmsg.template_tools @@ -55,44 +57,93 @@ 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) + """ + 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) + """ + 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) + + +def copy_changed(inputdir, outputdir): + """ + Copies files from inputdir to outputdir if they don't exist in + ouputdir or if their content changed + """ + for f in os.listdir(inputdir): + fni = os.path.join(inputdir, f) + if os.path.isfile(fni): + # Check if f exists in outpoutdir, copy the file if not + fno = os.path.join(outputdir, f) + if not os.path.isfile(fno): + shutil.copy(fni, fno) + print("{0}: new header file".format(f)) + continue + # The file exists in inputdir and outputdir + # only copy if contents do not match + if not filecmp.cmp(fni, fno): + shutil.copy(fni, fno) + print("{0}: updated".format(f)) + continue + + print("{0}: unchanged".format(f)) + +def convert_dir_save(inputdir, outputdir, templatedir, temporarydir): + """ + Converts all .msg files in inputdir to uORB header files + Unchanged existing files are not overwritten. + """ + # Create new headers in temporary output directory + convert_dir(inputdir, temporarydir, templatedir) + + # Copy changed headers from temporary dir to output dir + copy_changed(temporarydir, outputdir) 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() + 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') + parser.add_argument('-t', dest='temporarydir', + help='temporary directory') + 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) + 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_save( + args.dir, + args.outputdir, + args.templatedir, + args.temporarydir) diff --git a/makefiles/setup.mk b/makefiles/setup.mk index 4bfa7a087..435c240bf 100644 --- a/makefiles/setup.mk +++ b/makefiles/setup.mk @@ -37,13 +37,14 @@ # Some useful paths. # # Note that in general we always keep directory paths with the separator -# at the end, and join paths without explicit separators. This reduces +# at the end, and join paths without explicit separators. This reduces # the number of duplicate slashes we have lying around in paths, # and is consistent with joining the results of $(dir) and $(notdir). # export PX4_INCLUDE_DIR = $(abspath $(PX4_BASE)/src/include)/ export PX4_MODULE_SRC = $(abspath $(PX4_BASE)/src)/ export PX4_LIB_DIR = $(abspath $(PX4_BASE)/src/lib)/ +export PX4_PLATFORMS_DIR = $(abspath $(PX4_BASE)/src/platforms)/ export PX4_MK_DIR = $(abspath $(PX4_BASE)/makefiles)/ export NUTTX_SRC = $(abspath $(PX4_BASE)/NuttX/nuttx)/ export MAVLINK_SRC = $(abspath $(PX4_BASE)/mavlink/include/mavlink/v1.0)/ @@ -61,7 +62,8 @@ export ARCHIVE_DIR = $(abspath $(PX4_BASE)/Archives)/ export INCLUDE_DIRS := $(PX4_MODULE_SRC) \ $(PX4_MODULE_SRC)/modules/ \ $(PX4_INCLUDE_DIR) \ - $(PX4_LIB_DIR) + $(PX4_LIB_DIR) \ + $(PX4_PLATFORMS_DIR) # # Tools diff --git a/src/examples/subscriber/module.mk b/src/examples/subscriber/module.mk index 481c36ea7..90b4d8ffc 100644 --- a/src/examples/subscriber/module.mk +++ b/src/examples/subscriber/module.mk @@ -37,6 +37,7 @@ MODULE_COMMAND = subscriber -SRCS = subscriber.cpp +SRCS = subscriber.cpp \ + subscriber_params.c MODULE_STACKSIZE = 2400 diff --git a/src/examples/subscriber/subscriber.cpp b/src/examples/subscriber/subscriber.cpp index a29f9ab93..68003b6df 100644 --- a/src/examples/subscriber/subscriber.cpp +++ b/src/examples/subscriber/subscriber.cpp @@ -26,6 +26,7 @@ */ #include <px4.h> +#include "subscriber_params.h" using namespace px4; @@ -74,6 +75,18 @@ PX4_MAIN_FUNCTION(subscriber) { */ px4::NodeHandle n; + /* Define parameters */ + px4_param_t p_sub_interv = PX4_PARAM_INIT(SUB_INTERV); + px4_param_t p_test_float = PX4_PARAM_INIT(SUB_TESTF); + + /* Read the parameter back for testing */ + int32_t sub_interval; + float test_float; + PX4_PARAM_GET(p_sub_interv, &sub_interval); + PX4_INFO("Param SUB_INTERV = %d", sub_interval); + PX4_PARAM_GET(p_test_float, &test_float); + PX4_INFO("Param SUB_TESTF = %.3f", (double)test_float); + /** * The subscribe() call is how you tell ROS that you want to receive messages * on a given topic. This invokes a call to the ROS @@ -89,7 +102,7 @@ PX4_MAIN_FUNCTION(subscriber) { * is the number of messages that will be buffered up before beginning to throw * away the oldest ones. */ - PX4_SUBSCRIBE(n, rc_channels, rc_channels_callback, 100); + PX4_SUBSCRIBE(n, rc_channels, rc_channels_callback, sub_interval); PX4_SUBSCRIBE(n, rc_channels, rc_channels_callback2, 1000); PX4_SUBSCRIBE(n, rc_channels, RCHandler::callback, rchandler, 1000); PX4_INFO("subscribed"); diff --git a/src/examples/subscriber/subscriber_params.c b/src/examples/subscriber/subscriber_params.c new file mode 100644 index 000000000..a43817b5b --- /dev/null +++ b/src/examples/subscriber/subscriber_params.c @@ -0,0 +1,56 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file subscriber_params.c + * Parameters for the subscriber example + * + * @author Thomas Gubler <thomasgubler@gmail.com> + */ + +#include <px4_defines.h> +#include "subscriber_params.h" + +/** + * Interval of one subscriber in the example in ms + * + * @group Subscriber Example + */ +PX4_PARAM_DEFINE_INT32(SUB_INTERV); + +/** + * Float Demonstration Parameter in the Example + * + * @group Subscriber Example + */ +PX4_PARAM_DEFINE_FLOAT(SUB_TESTF); diff --git a/src/examples/subscriber/subscriber_params.h b/src/examples/subscriber/subscriber_params.h new file mode 100644 index 000000000..f6f1d6ba0 --- /dev/null +++ b/src/examples/subscriber/subscriber_params.h @@ -0,0 +1,43 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file subscriber_params.h + * Default parameters for the subscriber example + * + * @author Thomas Gubler <thomasgubler@gmail.com> + */ +#pragma once + +#define PARAM_SUB_INTERV_DEFAULT 100 +#define PARAM_SUB_TESTF_DEFAULT 3.14f diff --git a/src/include/px4.h b/src/include/px4.h index 22d661b17..ca702d63c 100644 --- a/src/include/px4.h +++ b/src/include/px4.h @@ -39,26 +39,7 @@ #pragma once -#include <stdbool.h> - -#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) -/* - * Building for running within the ROS environment - */ -#include "ros/ros.h" -#include "px4/rc_channels.h" - -#else -/* - * Building for NuttX - */ -#include <nuttx/config.h> -#include <uORB/uORB.h> -#include <uORB/topics/rc_channels.h> -#include <systemlib/err.h> - -#endif - +#include "../platforms/px4_includes.h" #include "../platforms/px4_defines.h" #include "../platforms/px4_middleware.h" #include "../platforms/px4_nodehandle.h" diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 2dd57940d..af7188f32 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -38,36 +38,99 @@ */ #pragma once +/* Get the name of the default value fiven the param name */ +#define PX4_PARAM_DEFAULT_VALUE_NAME(_name) PARAM_##_name##_DEFAULT + +/* Shortcuts to define parameters when the default value is defined according to PX4_PARAM_DEFAULT_VALUE_NAME */ +#define PX4_PARAM_DEFINE_INT32(_name) PARAM_DEFINE_INT32(_name, PX4_PARAM_DEFAULT_VALUE_NAME(_name)) +#define PX4_PARAM_DEFINE_FLOAT(_name) PARAM_DEFINE_FLOAT(_name, PX4_PARAM_DEFAULT_VALUE_NAME(_name)) + #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) /* * Building for running within the ROS environment */ #define __EXPORT +#include "ros/ros.h" +/* Main entry point */ #define PX4_MAIN_FUNCTION(_prefix) int main(int argc, char **argv) + +/* Print/output wrappers */ #define PX4_WARN ROS_WARN #define PX4_INFO ROS_INFO + +/* Topic Handle */ #define PX4_TOPIC(_name) #_name + +/* Topic type */ #define PX4_TOPIC_T(_name) _name + +/* Subscribe and providing a class method as callback (do not use directly, use PX4_SUBSCRIBE instead) */ #define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _obj, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), &_cbf, &_obj); +/* Subscribe and providing a function as callback (do not use directly, use PX4_SUBSCRIBE instead) */ #define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), _cbf); + +/* Parameter handle datatype */ +typedef const char *px4_param_t; + +/* Helper fucntions to set ROS params, only int and float supported */ +static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, int value) +{ + ros::param::set(name, value); + return (px4_param_t)name; +}; +static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, float value) +{ + ros::param::set(name, value); + return (px4_param_t)name; +}; + +/* Initialize a param, in case of ROS the parameter is sent to the parameter server here */ +#define PX4_PARAM_INIT(_name) PX4_ROS_PARAM_SET(#_name, PX4_PARAM_DEFAULT_VALUE_NAME(_name)) + +/* Get value of parameter */ +#define PX4_PARAM_GET(_handle, _destpt) ros::param::get(_handle, *_destpt) + #else /* * Building for NuttX */ +#include <platforms/px4_includes.h> +/* Main entry point */ #define PX4_MAIN_FUNCTION(_prefix) extern "C" __EXPORT int _prefix##_main(int argc, char *argv[]) -#define PX4_WARN warnx + +/* Print/output wrappers */ #define PX4_WARN warnx #define PX4_INFO warnx + +/* Topic Handle */ #define PX4_TOPIC(_name) ORB_ID(_name) + +/* Topic type */ #define PX4_TOPIC_T(_name) _name##_s + +/* Subscribe and providing a class method as callback (do not use directly, use PX4_SUBSCRIBE instead) */ #define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _obj, _interval) _nodehandle.subscribe<PX4_TOPIC_T(_name)>(PX4_TOPIC(_name), std::bind(&_cbf, _obj, std::placeholders::_1), _interval) +/* Subscribe and providing a function as callback (do not use directly, use PX4_SUBSCRIBE instead) */ #define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe<PX4_TOPIC_T(_name)>(PX4_TOPIC(_name), std::bind(&_cbf, std::placeholders::_1), _interval) + +/* Parameter handle datatype */ +typedef param_t px4_param_t; + +/* Initialize a param, get param handle */ +#define PX4_PARAM_INIT(_name) param_find(#_name) + +/* Get value of parameter */ +#define PX4_PARAM_GET(_handle, _destpt) param_get(_handle, _destpt) #endif -/* Overload the PX4_SUBSCRIBE macro to suppport methods and pure functions as callback */ +/* Shortcut for subscribing to topics + * Overload the PX4_SUBSCRIBE macro to suppport methods and pure functions as callback + */ #define PX4_GET_SUBSCRIBE(_1, _2, _3, _4, _5, NAME, ...) NAME #define PX4_SUBSCRIBE(...) PX4_GET_SUBSCRIBE(__VA_ARGS__, PX4_SUBSCRIBE_CBMETH, PX4_SUBSCRIBE_CBFUNC)(__VA_ARGS__) + +/* shortcut for advertising topics */ #define PX4_ADVERTISE(_nodehandle, _name) _nodehandle.advertise<PX4_TOPIC_T(_name)>(PX4_TOPIC(_name)) /* wrapper for 2d matrices */ diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h new file mode 100644 index 000000000..a3b59b766 --- /dev/null +++ b/src/platforms/px4_includes.h @@ -0,0 +1,61 @@ +/**************************************************************************** + * + * Copyright (c) 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. + * + ****************************************************************************/ + +/** + * @file px4_includes.h + * + * Includes headers depending on the build target + */ + +#pragma once + +#include <stdbool.h> + +#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) +/* + * Building for running within the ROS environment + */ +#include "ros/ros.h" +#include "px4/rc_channels.h" + +#else +/* + * Building for NuttX + */ +#include <nuttx/config.h> +#include <uORB/uORB.h> +#include <uORB/topics/rc_channels.h> +#include <systemlib/err.h> +#include <systemlib/param/param.h> + +#endif diff --git a/src/platforms/px4_middleware.h b/src/platforms/px4_middleware.h index dece72907..c1465b4b1 100644 --- a/src/platforms/px4_middleware.h +++ b/src/platforms/px4_middleware.h @@ -50,9 +50,15 @@ __EXPORT void init(int argc, char *argv[], const char *process_name); __EXPORT uint64_t get_time_micros(); #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) +/** + * Returns true if the app/task should continue to run + */ bool ok() { return ros::ok(); } #else extern bool task_should_exit; +/** + * Returns true if the app/task should continue to run + */ bool ok() { return !task_should_exit; } #endif @@ -60,8 +66,15 @@ class Rate { public: + /** + * Construct the Rate object and set rate + * @param rate_hz rate from which sleep time is calculated in Hz + */ explicit Rate(unsigned rate_hz) { sleep_interval = 1e6 / rate_hz; } + /** + * Sleep for 1/rate_hz s + */ void sleep() { usleep(sleep_interval); } private: diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index 415351756..5b7247b20 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -55,7 +55,6 @@ namespace px4 { -//XXX create abstract base class #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) class NodeHandle : private ros::NodeHandle @@ -67,42 +66,67 @@ public: _pubs() {} - ~NodeHandle() { + ~NodeHandle() + { //XXX empty lists }; - /* Constructor with callback to function */ + /** + * Subscribe with callback to function + * @param topic Name of the topic + * @param fb Callback, executed on receiving a new message + */ template<typename M> - Subscriber * subscribe(const char *topic, void(*fp)(M)) { + Subscriber *subscribe(const char *topic, void(*fp)(M)) + { ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, fp); - Subscriber * sub = new Subscriber(ros_sub); + Subscriber *sub = new Subscriber(ros_sub); _subs.push_back(sub); return sub; } - /* Constructor with callback to class method */ + + /** + * Subscribe with callback to class method + * @param topic Name of the topic + * @param fb Callback, executed on receiving a new message + */ template<typename M, typename T> - Subscriber * subscribe(const char *topic, void(T::*fp)(M), T *obj) { + Subscriber *subscribe(const char *topic, void(T::*fp)(M), T *obj) + { ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, fp, obj); - Subscriber * sub = new Subscriber(ros_sub); + Subscriber *sub = new Subscriber(ros_sub); _subs.push_back(sub); return sub; } + /** + * Advertise topic + * @param topic Name of the topic + */ template<typename M> - Publisher * advertise(const char *topic) { + Publisher *advertise(const char *topic) + { ros::Publisher ros_pub = ros::NodeHandle::advertise<M>(topic, kQueueSizeDefault); Publisher *pub = new Publisher(ros_pub); _pubs.push_back(pub); return pub; } + /** + * Calls all callback waiting to be called + */ + void spinOnce() { ros::spinOnce(); } + + /** + * Keeps calling callbacks for incomming messages, returns when module is terminated + */ void spin() { ros::spin(); } - void spinOnce() { ros::spinOnce(); } + private: - static const uint32_t kQueueSizeDefault = 1000; - std::list<Subscriber*> _subs; - std::list<Publisher*> _pubs; + static const uint32_t kQueueSizeDefault = 1000; /**< Size of queue for ROS */ + std::list<Subscriber *> _subs; /**< Subcriptions of node */ + std::list<Publisher *> _pubs; /**< Publications of node */ }; #else class __EXPORT NodeHandle @@ -116,27 +140,45 @@ public: ~NodeHandle() {}; + /** + * Subscribe with callback to function + * @param meta Describes the topic which nodehande should subscribe to + * @param callback Callback, executed on receiving a new message + * @param interval Minimal interval between calls to callback + */ + template<typename M> - Subscriber * subscribe(const struct orb_metadata *meta, - std::function<void(const M&)> callback, - unsigned interval) { + Subscriber *subscribe(const struct orb_metadata *meta, + std::function<void(const M &)> callback, + unsigned interval) + { SubscriberPX4<M> *sub_px4 = new SubscriberPX4<M>(meta, interval, callback, &_subs); /* Check if this is the smallest interval so far and update _sub_min_interval */ if (_sub_min_interval == nullptr || _sub_min_interval->getInterval() > sub_px4->getInterval()) { _sub_min_interval = sub_px4; } - return (Subscriber*)sub_px4; + + return (Subscriber *)sub_px4; } + /** + * Advertise topic + * @param meta Describes the topic which is advertised + */ template<typename M> - Publisher * advertise(const struct orb_metadata *meta) { + Publisher *advertise(const struct orb_metadata *meta) + { //XXX - Publisher * pub = new Publisher(meta, &_pubs); + Publisher *pub = new Publisher(meta, &_pubs); return pub; } - void spinOnce() { + /** + * Calls all callback waiting to be called + */ + void spinOnce() + { /* Loop through subscriptions, call callback for updated subscriptions */ uORB::SubscriptionNode *sub = _subs.getHead(); int count = 0; @@ -152,9 +194,14 @@ public: } } - void spin() { + /** + * Keeps calling callbacks for incomming messages, returns when module is terminated + */ + void spin() + { while (ok()) { const int timeout_ms = 100; + /* Only continue in the loop if the nodehandle has subscriptions */ if (_sub_min_interval == nullptr) { usleep(timeout_ms * 1000); @@ -165,6 +212,7 @@ public: struct pollfd pfd; pfd.fd = _sub_min_interval->getHandle(); pfd.events = POLLIN; + if (poll(&pfd, 1, timeout_ms) <= 0) { /* timed out */ continue; @@ -175,9 +223,9 @@ public: } private: static const uint16_t kMaxSubscriptions = 100; - List<uORB::SubscriptionNode*> _subs; - List<uORB::PublicationNode*> _pubs; - uORB::SubscriptionNode* _sub_min_interval; /**< Points to the sub wtih the smallest interval + List<uORB::SubscriptionNode *> _subs; /**< Subcriptions of node */ + List<uORB::PublicationNode *> _pubs; /**< Publications of node */ + uORB::SubscriptionNode *_sub_min_interval; /**< Points to the sub wtih the smallest interval of all Subscriptions in _subs*/ }; #endif diff --git a/src/platforms/px4_publisher.h b/src/platforms/px4_publisher.h index 9ce211d25..cb15eeb58 100644 --- a/src/platforms/px4_publisher.h +++ b/src/platforms/px4_publisher.h @@ -52,33 +52,54 @@ namespace px4 class Publisher { public: + /** + * Construct Publisher by providing a ros::Publisher + * @param ros_pub the ros publisher which will be used to perform the publications + */ Publisher(ros::Publisher ros_pub) : _ros_pub(ros_pub) {} + ~Publisher() {}; + + /** Publishes msg + * @param msg the message which is published to the topic + */ template<typename M> int publish(const M &msg) { _ros_pub.publish(msg); return 0; } private: - ros::Publisher _ros_pub; + ros::Publisher _ros_pub; /**< Handle to the ros publisher */ }; #else class Publisher : public uORB::PublicationNode { public: + /** + * Construct Publisher by providing orb meta data + * @param meta orb metadata for the topic which is used + * @param list publisher is added to this list + */ Publisher(const struct orb_metadata *meta, - List<uORB::PublicationNode *> * list) : + List<uORB::PublicationNode *> *list) : uORB::PublicationNode(meta, list) {} + ~Publisher() {}; + + /** Publishes msg + * @param msg the message which is published to the topic + */ template<typename M> - int publish(const M &msg) { - uORB::PublicationBase::update((void*)&msg); + int publish(const M &msg) + { + uORB::PublicationBase::update((void *)&msg); return 0; } - void update() { - //XXX list traversal callback, needed? - } ; + /** + * Empty callback for list traversal + */ + void update() {} ; }; #endif } diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h index 6e0ef8aed..e995c6e49 100644 --- a/src/platforms/px4_subscriber.h +++ b/src/platforms/px4_subscriber.h @@ -54,15 +54,24 @@ namespace px4 class Subscriber { public: + /** + * Construct Subscriber by providing a ros::Subscriber + * @param ros_sub the ros subscriber which will be used to perform the publications + */ Subscriber(ros::Subscriber ros_sub) : _ros_sub(ros_sub) {} + ~Subscriber() {}; private: - ros::Subscriber _ros_sub; + ros::Subscriber _ros_sub; /**< Handle to ros subscriber */ }; + #else -// typedef std::function<void(int)> CallbackFunction; + +/** + * Subscriber class which is used by nodehandle when building for NuttX + */ class Subscriber { public: @@ -71,24 +80,41 @@ public: private: }; +/** + * Subscriber class that is templated with the uorb subscription message type + */ template<typename M> class SubscriberPX4 : public Subscriber, public uORB::Subscription<M> { public: + /** + * Construct SubscriberPX4 by providing orb meta data + * @param meta orb metadata for the topic which is used + * @param callback Callback, executed on receiving a new message + * @param interval Minimal interval between calls to callback + * @param list subscriber is added to this list + */ SubscriberPX4(const struct orb_metadata *meta, - unsigned interval, - std::function<void(const M&)> callback, - List<uORB::SubscriptionNode *> * list) : + unsigned interval, + std::function<void(const M &)> callback, + List<uORB::SubscriptionNode *> *list) : Subscriber(), uORB::Subscription<M>(meta, interval, list), _callback(callback) //XXX store callback {} + ~SubscriberPX4() {}; - void update() { + /** + * Update Subscription + * Invoked by the list traversal in NodeHandle::spinOnce + * If new data is available the callback is called + */ + void update() + { if (!uORB::Subscription<M>::updated()) { /* Topic not updated, do not call callback */ return; @@ -102,7 +128,8 @@ public: }; private: - std::function<void(const M&)> _callback; + std::function<void(const M &)> _callback; /**< Callback handle, + called when new data is available */ }; #endif |