diff options
Diffstat (limited to 'src/platforms/px4_subscriber.h')
-rw-r--r-- | src/platforms/px4_subscriber.h | 191 |
1 files changed, 101 insertions, 90 deletions
diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h index aaacf9e48..6ca35b173 100644 --- a/src/platforms/px4_subscriber.h +++ b/src/platforms/px4_subscriber.h @@ -39,6 +39,7 @@ #pragma once #include <functional> +#include <type_traits> #if defined(__PX4_ROS) /* includes when building for ros */ @@ -56,139 +57,147 @@ namespace px4 /** * Untemplated subscriber base class * */ -class SubscriberBase +class __EXPORT SubscriberBase { public: SubscriberBase() {}; - ~SubscriberBase() {}; + virtual ~SubscriberBase() {}; }; /** * Subscriber class which is used by nodehandle */ -template<typename M> -class Subscriber : +template<typename T> +class __EXPORT Subscriber : public SubscriberBase { public: Subscriber() : - SubscriberBase() + SubscriberBase(), + _msg_current() {}; - ~Subscriber() {}; + + virtual ~Subscriber() {} /* Accessors*/ /** * Get the last message value */ - virtual M get() = 0; + virtual T& get() {return _msg_current;} /** - * Get void pointer to last message value + * Get the last native message value */ - virtual void *get_void_ptr() = 0; + virtual decltype(((T*)nullptr)->data()) data() + { + return _msg_current.data(); + } + +protected: + T _msg_current; /**< Current Message value */ }; #if defined(__PX4_ROS) /** * Subscriber class that is templated with the ros n message type */ -template<typename M> +template<typename T> class SubscriberROS : - public Subscriber<M> + public Subscriber<T> { - friend class NodeHandle; - public: /** - * Construct Subscriber by providing a callback function - */ - SubscriberROS(std::function<void(const M &)> cbf) : - Subscriber<M>(), - _ros_sub(), - _cbf(cbf), - _msg_current() - {} - - /** * Construct Subscriber without a callback function */ - SubscriberROS() : - Subscriber<M>(), - _ros_sub(), + SubscriberROS(ros::NodeHandle *rnh) : + px4::Subscriber<T>(), _cbf(NULL), - _msg_current() + _ros_sub(rnh->subscribe(T::handle(), kQueueSizeDefault, &SubscriberROS<T>::callback, this)) {} - - ~SubscriberROS() {}; - - /* Accessors*/ /** - * Get the last message value - */ - M get() { return _msg_current; } - /** - * Get void pointer to last message value + * Construct Subscriber by providing a callback function */ - void *get_void_ptr() { return (void *)&_msg_current; } + SubscriberROS(ros::NodeHandle *rnh, std::function<void(const T &)> cbf) : + _cbf(cbf), + _ros_sub(rnh->subscribe(T::handle(), kQueueSizeDefault, &SubscriberROS<T>::callback, this)) + {} + + virtual ~SubscriberROS() {}; protected: + static const uint32_t kQueueSizeDefault = 1; /**< Size of queue for ROS */ + /** * Called on topic update, saves the current message and then calls the provided callback function + * needs to use the native type as it is called by ROS */ - void callback(const M &msg) + void callback(const typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &msg) { /* Store data */ - _msg_current = msg; + this->_msg_current.data() = msg; /* Call callback */ if (_cbf != NULL) { - _cbf(msg); + _cbf(this->get()); } } - /** - * Saves the ros subscriber to keep ros subscription alive - */ - void set_ros_sub(ros::Subscriber ros_sub) - { - _ros_sub = ros_sub; - } - ros::Subscriber _ros_sub; /**< Handle to ros subscriber */ - std::function<void(const M &)> _cbf; /**< Callback that the user provided on the subscription */ - M _msg_current; /**< Current Message value */ + std::function<void(const T &)> _cbf; /**< Callback that the user provided on the subscription */ }; #else // Building for NuttX +/** + * Because we maintain a list of subscribers we need a node class + */ +class __EXPORT SubscriberNode : + public ListNode<SubscriberNode *> +{ +public: + SubscriberNode(unsigned interval) : + ListNode(), + _interval(interval) + {} + + virtual ~SubscriberNode() {} + + virtual void update() = 0; + + virtual int getUORBHandle() = 0; + + unsigned get_interval() { return _interval; } + +protected: + unsigned _interval; + +}; /** * Subscriber class that is templated with the uorb subscription message type */ -template<typename M> -class SubscriberUORB : - public Subscriber<M>, - public uORB::Subscription<M> +template<typename T> +class __EXPORT SubscriberUORB : + public Subscriber<T>, + public SubscriberNode { public: /** * Construct SubscriberUORB by providing orb meta data without callback - * @param meta orb metadata for the topic which is used * @param interval Minimal interval between calls to callback - * @param list subscriber is added to this list */ - SubscriberUORB(const struct orb_metadata *meta, - unsigned interval, - List<uORB::SubscriptionNode *> *list) : - Subscriber<M>(), - uORB::Subscription<M>(meta, interval, list) + SubscriberUORB(unsigned interval) : + SubscriberNode(interval), + _uorb_sub(new uORB::SubscriptionBase(T::handle(), interval)) {} - ~SubscriberUORB() {}; + virtual ~SubscriberUORB() { + delete _uorb_sub; + }; /** * Update Subscription @@ -196,47 +205,50 @@ public: */ virtual void update() { - if (!uORB::Subscription<M>::updated()) { - /* Topic not updated */ + if (!_uorb_sub->updated()) { + /* Topic not updated, do not call callback */ return; } - /* get latest data */ - uORB::Subscription<M>::update(); + _uorb_sub->update(get_void_ptr()); }; /* Accessors*/ - /** - * Get the last message value - */ - M get() { return uORB::Subscription<M>::getData(); } + int getUORBHandle() { return _uorb_sub->getHandle(); } + +protected: + uORB::SubscriptionBase * _uorb_sub; /**< Handle to the subscription */ + + typename std::remove_reference<decltype(((T*)nullptr)->data())>::type getUORBData() + { + return (typename std::remove_reference<decltype(((T*)nullptr)->data())>::type)*_uorb_sub; + } + /** * Get void pointer to last message value */ - void *get_void_ptr() { return uORB::Subscription<M>::getDataVoidPtr(); } + void *get_void_ptr() { return (void *)&(this->_msg_current.data()); } + }; -template<typename M> -class SubscriberUORBCallback : - public SubscriberUORB<M> +//XXX reduce to one class with overloaded constructor? +template<typename T> +class __EXPORT SubscriberUORBCallback : + public SubscriberUORB<T> { public: /** * Construct SubscriberUORBCallback 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 cbf Callback, executed on receiving a new message * @param interval Minimal interval between calls to callback - * @param list subscriber is added to this list */ - SubscriberUORBCallback(const struct orb_metadata *meta, - unsigned interval, - std::function<void(const M &)> callback, - List<uORB::SubscriptionNode *> *list) : - SubscriberUORB<M>(meta, interval, list), - _callback(callback) + SubscriberUORBCallback(unsigned interval, + std::function<void(const T &)> cbf) : + SubscriberUORB<T>(interval), + _cbf(cbf) {} - ~SubscriberUORBCallback() {}; + virtual ~SubscriberUORBCallback() {}; /** * Update Subscription @@ -245,28 +257,27 @@ public: */ virtual void update() { - if (!uORB::Subscription<M>::updated()) { + if (!this->_uorb_sub->updated()) { /* Topic not updated, do not call callback */ return; } /* get latest data */ - uORB::Subscription<M>::update(); + this->_uorb_sub->update(this->get_void_ptr()); /* Check if there is a callback */ - if (_callback == nullptr) { + if (_cbf == nullptr) { return; } /* Call callback which performs actions based on this data */ - _callback(uORB::Subscription<M>::getData()); + _cbf(Subscriber<T>::get()); }; protected: - std::function<void(const M &)> _callback; /**< Callback handle, - called when new data is available */ + std::function<void(const T &)> _cbf; /**< Callback that the user provided on the subscription */ }; #endif |