From 905913986a03cdb31d056b278cadc2d6f6421e38 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 3 Dec 2014 07:43:08 +0100 Subject: [PATCH 01/11] update comments --- src/platforms/px4_nodehandle.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index 4153517565..cf47fec592 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 @@ -71,7 +70,7 @@ public: //XXX empty lists }; - /* Constructor with callback to function */ + /* Subscribe with callback to function */ template Subscriber * subscribe(const char *topic, void(*fp)(M)) { ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, fp); @@ -79,7 +78,8 @@ public: _subs.push_back(sub); return sub; } - /* Constructor with callback to class method */ + + /* Subscribe with callback to class method */ template Subscriber * subscribe(const char *topic, void(T::*fp)(M), T *obj) { ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, fp, obj); From 6a99b04fb74f6da3faea54c93d234a9b57d7bd0e Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 3 Dec 2014 12:31:37 +0100 Subject: [PATCH 02/11] add parameter wrapper macros for ros --- src/examples/subscriber/subscriber.cpp | 8 ++++++++ src/platforms/px4_defines.h | 2 ++ 2 files changed, 10 insertions(+) diff --git a/src/examples/subscriber/subscriber.cpp b/src/examples/subscriber/subscriber.cpp index a29f9ab934..a8ecd4a7db 100644 --- a/src/examples/subscriber/subscriber.cpp +++ b/src/examples/subscriber/subscriber.cpp @@ -74,6 +74,14 @@ PX4_MAIN_FUNCTION(subscriber) { */ px4::NodeHandle n; + /* Define a parameter */ + PX4_PARAM_INIT("SUB_INTERV", 100); + + /* Read the parameter back for testing */ + int32_t sub_interval; + PX4_PARAM_GET("SUB_INTERV", &sub_interval); + PX4_INFO("Param SUB_INTERV = %d", sub_interval); + /** * 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 diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 2dd57940d1..2d9051aaea 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -51,6 +51,8 @@ #define PX4_TOPIC_T(_name) _name #define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _obj, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), &_cbf, &_obj); #define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), _cbf); +#define PX4_PARAM_INIT(_name, _default) ros::param::set(_name, _default); +#define PX4_PARAM_GET(_name, _destpt) ros::param::get(_name, *_destpt) #else /* * Building for NuttX From 1c79f0cef1f21cff7935ddd7caf048fd96991eea Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 3 Dec 2014 14:43:03 +0100 Subject: [PATCH 03/11] improve param wrapper for ros, prepare for px4 --- src/examples/subscriber/subscriber.cpp | 12 ++++++++---- src/include/px4.h | 1 + src/platforms/px4_defines.h | 17 +++++++++++++++-- src/platforms/px4_nodehandle.h | 1 + 4 files changed, 25 insertions(+), 6 deletions(-) diff --git a/src/examples/subscriber/subscriber.cpp b/src/examples/subscriber/subscriber.cpp index a8ecd4a7db..e52d661a99 100644 --- a/src/examples/subscriber/subscriber.cpp +++ b/src/examples/subscriber/subscriber.cpp @@ -74,13 +74,17 @@ PX4_MAIN_FUNCTION(subscriber) { */ px4::NodeHandle n; - /* Define a parameter */ - PX4_PARAM_INIT("SUB_INTERV", 100); + /* Define parameters */ + px4_param_t p_sub_interv = PX4_PARAM_INIT("SUB_INTERV", 100); + px4_param_t p_test_float = PX4_PARAM_INIT("SUB_TESTF", 3.14f); /* Read the parameter back for testing */ int32_t sub_interval; - PX4_PARAM_GET("SUB_INTERV", &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 = %f", (double)test_float); /** * The subscribe() call is how you tell ROS that you want to receive messages @@ -97,7 +101,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/include/px4.h b/src/include/px4.h index 22d661b17c..2d5d259648 100644 --- a/src/include/px4.h +++ b/src/include/px4.h @@ -56,6 +56,7 @@ #include #include #include +#include #endif diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 2d9051aaea..a10df858a8 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -51,8 +51,18 @@ #define PX4_TOPIC_T(_name) _name #define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _obj, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), &_cbf, &_obj); #define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), _cbf); -#define PX4_PARAM_INIT(_name, _default) ros::param::set(_name, _default); -#define PX4_PARAM_GET(_name, _destpt) ros::param::get(_name, *_destpt) +typedef const std::string px4_param_t; +static inline px4_param_t ROS_PARAM_SET(const std::string &name, int32_t value) { + ros::param::set(name, value); + return (px4_param_t)name; +}; +static inline px4_param_t ROS_PARAM_SET(const std::string &name, float value) { + ros::param::set(name, value); + return (px4_param_t)name; +}; +#define PX4_PARAM_INIT(_name, _default) ROS_PARAM_SET(_name, _default) +// #define PX4_PARAM_INIT(_name, _default) ros::param::set(_name, _default) +#define PX4_PARAM_GET(_handle, _destpt) ros::param::get(_handle, *_destpt) #else /* * Building for NuttX @@ -65,6 +75,9 @@ #define PX4_TOPIC_T(_name) _name##_s #define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _obj, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), std::bind(&_cbf, _obj, std::placeholders::_1), _interval) #define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), std::bind(&_cbf, std::placeholders::_1), _interval) +typedef param_t px4_param_t +#define PX4_PARAM_INIT(_name, _default) param_find(_name) +#define PX4_PARAM_GET(_handle, _destpt) param_get(_handle, _destpt) #endif /* Overload the PX4_SUBSCRIBE macro to suppport methods and pure functions as callback */ diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index cf47fec592..a2775d69ab 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -99,6 +99,7 @@ public: void spin() { ros::spin(); } void spinOnce() { ros::spinOnce(); } + private: static const uint32_t kQueueSizeDefault = 1000; std::list _subs; From 924350a5de1a609e470618ef78212bf7b0044c33 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 3 Dec 2014 15:11:27 +0100 Subject: [PATCH 04/11] bring up param wrapper for px4, moved global include file --- src/examples/subscriber/module.mk | 3 +- src/examples/subscriber/subscriber.cpp | 2 +- src/examples/subscriber/subscriber_params.c | 55 +++++++++++++++++++ src/include/px4.h | 22 +------- src/platforms/px4_defines.h | 4 +- src/platforms/px4_includes.h | 61 +++++++++++++++++++++ 6 files changed, 123 insertions(+), 24 deletions(-) create mode 100644 src/examples/subscriber/subscriber_params.c create mode 100644 src/platforms/px4_includes.h diff --git a/src/examples/subscriber/module.mk b/src/examples/subscriber/module.mk index 481c36ea70..90b4d8ffc5 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 e52d661a99..cc9b7a7942 100644 --- a/src/examples/subscriber/subscriber.cpp +++ b/src/examples/subscriber/subscriber.cpp @@ -84,7 +84,7 @@ PX4_MAIN_FUNCTION(subscriber) { 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 = %f", (double)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 diff --git a/src/examples/subscriber/subscriber_params.c b/src/examples/subscriber/subscriber_params.c new file mode 100644 index 0000000000..e28bfbc363 --- /dev/null +++ b/src/examples/subscriber/subscriber_params.c @@ -0,0 +1,55 @@ +/**************************************************************************** + * + * 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 + */ + +#include + +/** + * Interval of one subscriber in the example in ms + * + * @group Subscriber Example + */ +PARAM_DEFINE_INT32(SUB_INTERV, 100); + +/** + * Float Demonstration Parameter in the Example + * + * @group Subscriber Example + */ +PARAM_DEFINE_FLOAT(SUB_TESTF, 3.14f); diff --git a/src/include/px4.h b/src/include/px4.h index 2d5d259648..ca702d63ce 100644 --- a/src/include/px4.h +++ b/src/include/px4.h @@ -39,27 +39,7 @@ #pragma once -#include - -#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 -#include -#include -#include -#include - -#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 a10df858a8..a715075108 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -63,10 +63,12 @@ static inline px4_param_t ROS_PARAM_SET(const std::string &name, float value) { #define PX4_PARAM_INIT(_name, _default) ROS_PARAM_SET(_name, _default) // #define PX4_PARAM_INIT(_name, _default) ros::param::set(_name, _default) #define PX4_PARAM_GET(_handle, _destpt) ros::param::get(_handle, *_destpt) + #else /* * Building for NuttX */ +#include #define PX4_MAIN_FUNCTION(_prefix) extern "C" __EXPORT int _prefix##_main(int argc, char *argv[]) #define PX4_WARN warnx #define PX4_WARN warnx @@ -75,7 +77,7 @@ static inline px4_param_t ROS_PARAM_SET(const std::string &name, float value) { #define PX4_TOPIC_T(_name) _name##_s #define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _obj, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), std::bind(&_cbf, _obj, std::placeholders::_1), _interval) #define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), std::bind(&_cbf, std::placeholders::_1), _interval) -typedef param_t px4_param_t +typedef param_t px4_param_t; #define PX4_PARAM_INIT(_name, _default) param_find(_name) #define PX4_PARAM_GET(_handle, _destpt) param_get(_handle, _destpt) #endif diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h new file mode 100644 index 0000000000..a3b59b766e --- /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 + +#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 +#include +#include +#include +#include + +#endif From c2e2b3d52fa7bcfbbec110b5555d2c437657f118 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 3 Dec 2014 17:04:15 +0100 Subject: [PATCH 05/11] make param wrapper macros compatible for px4 and ros, needs cleanup --- makefiles/setup.mk | 6 ++- src/examples/subscriber/subscriber.cpp | 5 ++- src/examples/subscriber/subscriber_params.c | 7 ++-- src/examples/subscriber/subscriber_params.h | 43 +++++++++++++++++++++ src/platforms/px4_defines.h | 21 +++++++--- 5 files changed, 70 insertions(+), 12 deletions(-) create mode 100644 src/examples/subscriber/subscriber_params.h diff --git a/makefiles/setup.mk b/makefiles/setup.mk index 4bfa7a0876..435c240bfa 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/subscriber.cpp b/src/examples/subscriber/subscriber.cpp index cc9b7a7942..68003b6dfc 100644 --- a/src/examples/subscriber/subscriber.cpp +++ b/src/examples/subscriber/subscriber.cpp @@ -26,6 +26,7 @@ */ #include +#include "subscriber_params.h" using namespace px4; @@ -75,8 +76,8 @@ PX4_MAIN_FUNCTION(subscriber) { px4::NodeHandle n; /* Define parameters */ - px4_param_t p_sub_interv = PX4_PARAM_INIT("SUB_INTERV", 100); - px4_param_t p_test_float = PX4_PARAM_INIT("SUB_TESTF", 3.14f); + 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; diff --git a/src/examples/subscriber/subscriber_params.c b/src/examples/subscriber/subscriber_params.c index e28bfbc363..0d36f5021d 100644 --- a/src/examples/subscriber/subscriber_params.c +++ b/src/examples/subscriber/subscriber_params.c @@ -38,18 +38,19 @@ * @author Thomas Gubler */ -#include +#include +#include "subscriber_params.h" /** * Interval of one subscriber in the example in ms * * @group Subscriber Example */ -PARAM_DEFINE_INT32(SUB_INTERV, 100); +PARAM_DEFINE_INT32(SUB_INTERV, PX4_PARAM_DEFAULT_VALUE_NAME(SUB_INTERV)); /** * Float Demonstration Parameter in the Example * * @group Subscriber Example */ -PARAM_DEFINE_FLOAT(SUB_TESTF, 3.14f); +PARAM_DEFINE_FLOAT(SUB_TESTF, PX4_PARAM_DEFAULT_VALUE_NAME(SUB_TESTF)); diff --git a/src/examples/subscriber/subscriber_params.h b/src/examples/subscriber/subscriber_params.h new file mode 100644 index 0000000000..3f3ff5bce8 --- /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 + */ +#pragma once + +#define SUB_INTERV_DEFAULT_VALUE 100 +#define SUB_TESTF_DEFAULT_VALUE 3.14f diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index a715075108..6760f57ead 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -39,11 +39,15 @@ #pragma once +#define PX4_PARAM_DEFAULT_VALUE_NAME(_name) _name##_DEFAULT_VALUE + + #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) /* * Building for running within the ROS environment */ #define __EXPORT +#include "ros/ros.h" #define PX4_MAIN_FUNCTION(_prefix) int main(int argc, char **argv) #define PX4_WARN ROS_WARN #define PX4_INFO ROS_INFO @@ -51,18 +55,21 @@ #define PX4_TOPIC_T(_name) _name #define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _obj, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), &_cbf, &_obj); #define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), _cbf); -typedef const std::string px4_param_t; -static inline px4_param_t ROS_PARAM_SET(const std::string &name, int32_t value) { +typedef const char* px4_param_t; +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 ROS_PARAM_SET(const std::string &name, float value) { +static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, float value) { ros::param::set(name, value); return (px4_param_t)name; }; -#define PX4_PARAM_INIT(_name, _default) ROS_PARAM_SET(_name, _default) +// #define PARAM_DEFINE_INT32(_name, _default) static const int PX4_ROS_PARAM_DEFAULT_VALUE_NAME(_name) = _default; +// #define PARAM_DEFINE_FLOAT(_name, _default) static const float PX4_ROS_PARAM_DEFAULT_VALUE_NAME(_name) = _default; +#define PX4_PARAM_INIT(_name) PX4_ROS_PARAM_SET(#_name, PX4_PARAM_DEFAULT_VALUE_NAME(_name)) // #define PX4_PARAM_INIT(_name, _default) ros::param::set(_name, _default) #define PX4_PARAM_GET(_handle, _destpt) ros::param::get(_handle, *_destpt) +#define PX4_PARAM_INT32_T int //XXX #else /* @@ -78,8 +85,9 @@ static inline px4_param_t ROS_PARAM_SET(const std::string &name, float value) { #define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _obj, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), std::bind(&_cbf, _obj, std::placeholders::_1), _interval) #define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), std::bind(&_cbf, std::placeholders::_1), _interval) typedef param_t px4_param_t; -#define PX4_PARAM_INIT(_name, _default) param_find(_name) +#define PX4_PARAM_INIT(_name) param_find(#_name) #define PX4_PARAM_GET(_handle, _destpt) param_get(_handle, _destpt) +#define PX4_PARAM_INT32_T int32_t #endif /* Overload the PX4_SUBSCRIBE macro to suppport methods and pure functions as callback */ @@ -92,3 +100,6 @@ typedef param_t px4_param_t; /* wrapper for rotation matrices stored in arrays */ #define PX4_R(_array, _x, _y) PX4_ARRAY2D(_array, 3, _x, _y) + +// #define PX4_PARAM_DEFAULT_INT32(_name, _value) static const PX4_PARAM_INT32_T PX4_PARAM_DEFAULT_VALUE_NAME(_name) = _value; +// #define PX4_PARAM_DEFAULT_FLOAT(_name, _value) static const float PX4_PARAM_DEFAULT_VALUE_NAME(_name) = _value; From 83d97fd1c27728145e4869f957afcc7cae57ff8a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 4 Dec 2014 09:53:37 +0100 Subject: [PATCH 06/11] better support for param default values on ros and px4 --- src/examples/subscriber/subscriber_params.c | 4 ++-- src/examples/subscriber/subscriber_params.h | 4 ++-- src/platforms/px4_defines.h | 4 +++- 3 files changed, 7 insertions(+), 5 deletions(-) diff --git a/src/examples/subscriber/subscriber_params.c b/src/examples/subscriber/subscriber_params.c index 0d36f5021d..a43817b5b9 100644 --- a/src/examples/subscriber/subscriber_params.c +++ b/src/examples/subscriber/subscriber_params.c @@ -46,11 +46,11 @@ * * @group Subscriber Example */ -PARAM_DEFINE_INT32(SUB_INTERV, PX4_PARAM_DEFAULT_VALUE_NAME(SUB_INTERV)); +PX4_PARAM_DEFINE_INT32(SUB_INTERV); /** * Float Demonstration Parameter in the Example * * @group Subscriber Example */ -PARAM_DEFINE_FLOAT(SUB_TESTF, PX4_PARAM_DEFAULT_VALUE_NAME(SUB_TESTF)); +PX4_PARAM_DEFINE_FLOAT(SUB_TESTF); diff --git a/src/examples/subscriber/subscriber_params.h b/src/examples/subscriber/subscriber_params.h index 3f3ff5bce8..f6f1d6ba0c 100644 --- a/src/examples/subscriber/subscriber_params.h +++ b/src/examples/subscriber/subscriber_params.h @@ -39,5 +39,5 @@ */ #pragma once -#define SUB_INTERV_DEFAULT_VALUE 100 -#define SUB_TESTF_DEFAULT_VALUE 3.14f +#define PARAM_SUB_INTERV_DEFAULT 100 +#define PARAM_SUB_TESTF_DEFAULT 3.14f diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 6760f57ead..09338acabf 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -39,7 +39,9 @@ #pragma once -#define PX4_PARAM_DEFAULT_VALUE_NAME(_name) _name##_DEFAULT_VALUE +#define PX4_PARAM_DEFAULT_VALUE_NAME(_name) PARAM_##_name##_DEFAULT +#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__)) From 8e8f84bde0d2e15734d931ea38a7b294a06d7314 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 4 Dec 2014 10:39:24 +0100 Subject: [PATCH 07/11] uorb topic header generator: only create new files if the file content really changed --- Makefile | 5 +- Tools/px_generate_uorb_topic_headers.py | 121 +++++++++++++++++------- 2 files changed, 90 insertions(+), 36 deletions(-) diff --git a/Makefile b/Makefile index 7df8004a40..bdbc18be54 100644 --- a/Makefile +++ b/Makefile @@ -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 a738dcb7ef..2ddbd69847 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) From befe4c399e1f0f2864f91ddde4db585dabf3db99 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 4 Dec 2014 11:50:59 +0100 Subject: [PATCH 08/11] comments for px4 defines --- src/platforms/px4_defines.h | 53 +++++++++++++++++++++++++++++-------- 1 file changed, 42 insertions(+), 11 deletions(-) diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 09338acabf..5858a69d86 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -38,8 +38,10 @@ */ #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)) @@ -50,14 +52,28 @@ */ #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; @@ -66,35 +82,53 @@ static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, float value) { ros::param::set(name, value); return (px4_param_t)name; }; -// #define PARAM_DEFINE_INT32(_name, _default) static const int PX4_ROS_PARAM_DEFAULT_VALUE_NAME(_name) = _default; -// #define PARAM_DEFINE_FLOAT(_name, _default) static const float PX4_ROS_PARAM_DEFAULT_VALUE_NAME(_name) = _default; + +/* 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)) -// #define PX4_PARAM_INIT(_name, _default) ros::param::set(_name, _default) + +/* Get value of parameter */ #define PX4_PARAM_GET(_handle, _destpt) ros::param::get(_handle, *_destpt) -#define PX4_PARAM_INT32_T int //XXX #else /* * Building for NuttX */ #include +/* 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(_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(_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) -#define PX4_PARAM_INT32_T int32_t #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(_name)) /* wrapper for 2d matrices */ @@ -102,6 +136,3 @@ typedef param_t px4_param_t; /* wrapper for rotation matrices stored in arrays */ #define PX4_R(_array, _x, _y) PX4_ARRAY2D(_array, 3, _x, _y) - -// #define PX4_PARAM_DEFAULT_INT32(_name, _value) static const PX4_PARAM_INT32_T PX4_PARAM_DEFAULT_VALUE_NAME(_name) = _value; -// #define PX4_PARAM_DEFAULT_FLOAT(_name, _value) static const float PX4_PARAM_DEFAULT_VALUE_NAME(_name) = _value; From 0f094d35d5dd183e44148eb1acbd7b7d76fde669 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 4 Dec 2014 12:22:21 +0100 Subject: [PATCH 09/11] nodehandle: add documentation comments --- src/platforms/px4_nodehandle.h | 54 ++++++++++++++++++++++++++++------ 1 file changed, 45 insertions(+), 9 deletions(-) diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index a2775d69ab..1762e16568 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -70,7 +70,11 @@ public: //XXX empty lists }; - /* Subscribe 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 Subscriber * subscribe(const char *topic, void(*fp)(M)) { ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, fp); @@ -79,7 +83,11 @@ public: return sub; } - /* Subscribe 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 Subscriber * subscribe(const char *topic, void(T::*fp)(M), T *obj) { ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, fp, obj); @@ -88,6 +96,10 @@ public: return sub; } + /** + * Advertise topic + * @param topic Name of the topic + */ template Publisher * advertise(const char *topic) { ros::Publisher ros_pub = ros::NodeHandle::advertise(topic, kQueueSizeDefault); @@ -96,14 +108,21 @@ public: return pub; } - void spin() { ros::spin(); } - + /** + * 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(); } + + private: - static const uint32_t kQueueSizeDefault = 1000; - std::list _subs; - std::list _pubs; + static const uint32_t kQueueSizeDefault = 1000; /**< Size of queue for ROS */ + std::list _subs; /**< Subcriptions of node */ + std::list _pubs; /**< Publications of node */ }; #else class __EXPORT NodeHandle @@ -117,6 +136,13 @@ 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 Subscriber * subscribe(const struct orb_metadata *meta, std::function callback, @@ -130,6 +156,10 @@ public: return (Subscriber*)sub_px4; } + /** + * Advertise topic + * @param meta Describes the topic which is advertised + */ template Publisher * advertise(const struct orb_metadata *meta) { //XXX @@ -137,6 +167,9 @@ public: return pub; } + /** + * Calls all callback waiting to be called + */ void spinOnce() { /* Loop through subscriptions, call callback for updated subscriptions */ uORB::SubscriptionNode *sub = _subs.getHead(); @@ -153,6 +186,9 @@ public: } } + /** + * Keeps calling callbacks for incomming messages, returns when module is terminated + */ void spin() { while (ok()) { const int timeout_ms = 100; @@ -176,8 +212,8 @@ public: } private: static const uint16_t kMaxSubscriptions = 100; - List _subs; - List _pubs; + List _subs; /**< Subcriptions of node */ + List _pubs; /**< Publications of node */ uORB::SubscriptionNode* _sub_min_interval; /**< Points to the sub wtih the smallest interval of all Subscriptions in _subs*/ }; From e0bb713bb03c0aa79e69496c787c012a8e1b78d1 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 4 Dec 2014 13:06:38 +0100 Subject: [PATCH 10/11] more documentation comments --- src/platforms/px4_middleware.h | 13 +++++++++++++ src/platforms/px4_publisher.h | 28 ++++++++++++++++++++++++---- src/platforms/px4_subscriber.h | 32 +++++++++++++++++++++++++++++--- 3 files changed, 66 insertions(+), 7 deletions(-) diff --git a/src/platforms/px4_middleware.h b/src/platforms/px4_middleware.h index dece729070..c1465b4b1b 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_publisher.h b/src/platforms/px4_publisher.h index 9ce211d250..b67421066a 100644 --- a/src/platforms/px4_publisher.h +++ b/src/platforms/px4_publisher.h @@ -52,33 +52,53 @@ 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 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 * list) : uORB::PublicationNode(meta, list) {} + ~Publisher() {}; + + /** Publishes msg + * @param msg the message which is published to the topic + */ template 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 6e0ef8aed6..fdd0367d59 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 CallbackFunction; + +/** + * Subscriber class which is used by nodehandle when building for NuttX + */ class Subscriber { public: @@ -71,12 +80,22 @@ public: private: }; +/** + * Subscriber class that is templated with the uorb subscription message type + */ template class SubscriberPX4 : public Subscriber, public uORB::Subscription { 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 callback, @@ -86,8 +105,14 @@ public: _callback(callback) //XXX store callback {} + ~SubscriberPX4() {}; + /** + * Update Subscription + * Invoked by the list traversal in NodeHandle::spinOnce + * If new data is available the callback is called + */ void update() { if (!uORB::Subscription::updated()) { /* Topic not updated, do not call callback */ @@ -102,7 +127,8 @@ public: }; private: - std::function _callback; + std::function _callback; /**< Callback handle, + called when new data is available */ }; #endif From f4a326c2bf7af6eac86983cd65e66ff76e623e22 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 4 Dec 2014 13:20:12 +0100 Subject: [PATCH 11/11] platform headers: fix code style --- src/platforms/px4_defines.h | 8 +++--- src/platforms/px4_nodehandle.h | 49 +++++++++++++++++++++------------- src/platforms/px4_publisher.h | 7 ++--- src/platforms/px4_subscriber.h | 11 ++++---- 4 files changed, 45 insertions(+), 30 deletions(-) diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 5858a69d86..af7188f32c 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -71,14 +71,16 @@ #define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), _cbf); /* Parameter handle datatype */ -typedef const char* px4_param_t; +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) { +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) { +static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, float value) +{ ros::param::set(name, value); return (px4_param_t)name; }; diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index 1762e16568..5b7247b203 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -66,7 +66,8 @@ public: _pubs() {} - ~NodeHandle() { + ~NodeHandle() + { //XXX empty lists }; @@ -76,9 +77,10 @@ public: * @param fb Callback, executed on receiving a new message */ template - 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; } @@ -89,9 +91,10 @@ public: * @param fb Callback, executed on receiving a new message */ template - 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; } @@ -101,7 +104,8 @@ public: * @param topic Name of the topic */ template - Publisher * advertise(const char *topic) { + Publisher *advertise(const char *topic) + { ros::Publisher ros_pub = ros::NodeHandle::advertise(topic, kQueueSizeDefault); Publisher *pub = new Publisher(ros_pub); _pubs.push_back(pub); @@ -121,8 +125,8 @@ public: private: static const uint32_t kQueueSizeDefault = 1000; /**< Size of queue for ROS */ - std::list _subs; /**< Subcriptions of node */ - std::list _pubs; /**< Publications of node */ + std::list _subs; /**< Subcriptions of node */ + std::list _pubs; /**< Publications of node */ }; #else class __EXPORT NodeHandle @@ -144,16 +148,18 @@ public: */ template - Subscriber * subscribe(const struct orb_metadata *meta, - std::function callback, - unsigned interval) { + Subscriber *subscribe(const struct orb_metadata *meta, + std::function callback, + unsigned interval) + { SubscriberPX4 *sub_px4 = new SubscriberPX4(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; } /** @@ -161,16 +167,18 @@ public: * @param meta Describes the topic which is advertised */ template - 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; } /** * Calls all callback waiting to be called */ - void spinOnce() { + void spinOnce() + { /* Loop through subscriptions, call callback for updated subscriptions */ uORB::SubscriptionNode *sub = _subs.getHead(); int count = 0; @@ -189,9 +197,11 @@ public: /** * Keeps calling callbacks for incomming messages, returns when module is terminated */ - void spin() { + 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); @@ -202,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; @@ -212,9 +223,9 @@ public: } private: static const uint16_t kMaxSubscriptions = 100; - List _subs; /**< Subcriptions of node */ - List _pubs; /**< Publications of node */ - uORB::SubscriptionNode* _sub_min_interval; /**< Points to the sub wtih the smallest interval + List _subs; /**< Subcriptions of node */ + List _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 b67421066a..cb15eeb58f 100644 --- a/src/platforms/px4_publisher.h +++ b/src/platforms/px4_publisher.h @@ -80,7 +80,7 @@ public: * @param list publisher is added to this list */ Publisher(const struct orb_metadata *meta, - List * list) : + List *list) : uORB::PublicationNode(meta, list) {} @@ -90,8 +90,9 @@ public: * @param msg the message which is published to the topic */ template - int publish(const M &msg) { - uORB::PublicationBase::update((void*)&msg); + int publish(const M &msg) + { + uORB::PublicationBase::update((void *)&msg); return 0; } diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h index fdd0367d59..e995c6e49d 100644 --- a/src/platforms/px4_subscriber.h +++ b/src/platforms/px4_subscriber.h @@ -97,9 +97,9 @@ public: * @param list subscriber is added to this list */ SubscriberPX4(const struct orb_metadata *meta, - unsigned interval, - std::function callback, - List * list) : + unsigned interval, + std::function callback, + List *list) : Subscriber(), uORB::Subscription(meta, interval, list), _callback(callback) @@ -113,7 +113,8 @@ public: * Invoked by the list traversal in NodeHandle::spinOnce * If new data is available the callback is called */ - void update() { + void update() + { if (!uORB::Subscription::updated()) { /* Topic not updated, do not call callback */ return; @@ -127,7 +128,7 @@ public: }; private: - std::function _callback; /**< Callback handle, + std::function _callback; /**< Callback handle, called when new data is available */ };