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 /src/platforms | |
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
Diffstat (limited to 'src/platforms')
-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 |
6 files changed, 273 insertions, 40 deletions
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 |