diff options
Diffstat (limited to 'src')
-rw-r--r-- | src/examples/subscriber/subscriber_example.cpp | 19 | ||||
-rw-r--r-- | src/examples/subscriber/subscriber_example.h | 2 | ||||
-rw-r--r-- | src/platforms/nuttx/px4_messages/px4_rc_channels.h | 2 | ||||
-rw-r--r-- | src/platforms/px4_message.h | 3 | ||||
-rw-r--r-- | src/platforms/px4_nodehandle.h | 20 | ||||
-rw-r--r-- | src/platforms/px4_publisher.h | 4 | ||||
-rw-r--r-- | src/platforms/px4_subscriber.h | 93 |
7 files changed, 73 insertions, 70 deletions
diff --git a/src/examples/subscriber/subscriber_example.cpp b/src/examples/subscriber/subscriber_example.cpp index 1c8f4c62b..c662a07ad 100644 --- a/src/examples/subscriber/subscriber_example.cpp +++ b/src/examples/subscriber/subscriber_example.cpp @@ -43,8 +43,10 @@ using namespace px4; -void rc_channels_callback_function(const PX4_TOPIC_T(rc_channels) &msg) { - PX4_INFO("I heard: [%llu]", msg.timestamp_last_valid); +// void rc_channels_callback_function(const PX4_TOPIC_T(rc_channels) &msg) { +void rc_channels_callback_function(const px4_rc_channels &msg); +void rc_channels_callback_function(const px4_rc_channels &msg) { + PX4_INFO("I heard: [%llu]", msg.data().timestamp_last_valid); } SubscriberExample::SubscriberExample() : @@ -63,7 +65,8 @@ SubscriberExample::SubscriberExample() : /* Do some subscriptions */ /* Function */ // PX4_SUBSCRIBE(_n, rc_channels, rc_channels_callback_function, _interval); - _n.subscribe<px4_rc_channels>(rc_channels_callback_function); + _n.subscribe<px4_rc_channels>(rc_channels_callback_function); //ROS version + // _n.subscribe<px4_rc_channels>(std::bind(&rc_channels_callback_function, std::placeholders::_1)); UORB version // [> Class Method <] // PX4_SUBSCRIBE(_n, rc_channels, SubscriberExample::rc_channels_callback, this, 1000); @@ -77,8 +80,8 @@ SubscriberExample::SubscriberExample() : * This tutorial demonstrates simple receipt of messages over the PX4 middleware system. * Also the current value of the _sub_rc_chan subscription is printed */ -void SubscriberExample::rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg) { - PX4_INFO("Subscriber callback: [%llu], value of _sub_rc_chan: [%llu]", - msg.timestamp_last_valid, - _sub_rc_chan->get().data().timestamp_last_valid); -} +// void SubscriberExample::rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg) { + // PX4_INFO("Subscriber callback: [%llu], value of _sub_rc_chan: [%llu]", + // msg.timestamp_last_valid, + // _sub_rc_chan->get().data().timestamp_last_valid); +// } diff --git a/src/examples/subscriber/subscriber_example.h b/src/examples/subscriber/subscriber_example.h index b52ae991b..43dafe010 100644 --- a/src/examples/subscriber/subscriber_example.h +++ b/src/examples/subscriber/subscriber_example.h @@ -59,7 +59,7 @@ protected: // px4::PX4_SUBSCRIBER(rc_channels) * _sub_rc_chan; px4::Subscriber<px4_rc_channels> * _sub_rc_chan; - void rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg); + // void rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg); }; diff --git a/src/platforms/nuttx/px4_messages/px4_rc_channels.h b/src/platforms/nuttx/px4_messages/px4_rc_channels.h index bfca48469..cb1135eb9 100644 --- a/src/platforms/nuttx/px4_messages/px4_rc_channels.h +++ b/src/platforms/nuttx/px4_messages/px4_rc_channels.h @@ -6,7 +6,7 @@ namespace px4 { -class px4_rc_channels : +class __EXPORT px4_rc_channels : public PX4Message<rc_channels_s> { public: diff --git a/src/platforms/px4_message.h b/src/platforms/px4_message.h index 05fcf1140..c908f6fc6 100644 --- a/src/platforms/px4_message.h +++ b/src/platforms/px4_message.h @@ -49,7 +49,7 @@ namespace px4 { template <typename M> -class PX4Message +class __EXPORT PX4Message { // friend class NodeHandle; // #if defined(__PX4_ROS) @@ -69,6 +69,7 @@ public: virtual ~PX4Message() {}; virtual M& data() {return _data;} + virtual const M& data() const {return _data;} virtual PX4TopicHandle handle() = 0; private: M _data; diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index a40581239..2406a4a77 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -89,7 +89,8 @@ public: // return (Subscriber<M> *)sub; // } template<typename T> - Subscriber<T> *subscribe(void(*fp)(const typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &)) + // Subscriber<T> *subscribe(void(*fp)(const typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &)) + Subscriber<T> *subscribe(void(*fp)(const T &)) { SubscriberBase *sub = new SubscriberROS<T>(std::bind(fp, std::placeholders::_1)); ros::Subscriber ros_sub = ros::NodeHandle::subscribe((new T())->handle(), kQueueSizeDefault, @@ -240,17 +241,20 @@ public: */ template<typename T> - Subscriber<T> *subscribe(std::function<void(const typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &)> callback, unsigned interval=10) //XXX interval + // Subscriber<T> *subscribe(std::function<void(const typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &)> callback, unsigned interval=10) //XXX interval + // Subscriber<T> *subscribe(void(*fp)(const typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &), unsigned interval=10) //XXX interval + Subscriber<T> *subscribe(void(*fp)(const T &), unsigned interval=10) //XXX interval { const struct orb_metadata * meta = NULL; uORB::SubscriptionBase * uorb_sub = new uORB::SubscriptionBase(meta, interval); - SubscriberUORBCallback<T> *sub_px4 = new SubscriberUORBCallback<T>(uorb_sub, callback); + // SubscriberUORBCallback<T> *sub_px4 = new SubscriberUORBCallback<T>(uorb_sub, interval, callback); + SubscriberUORBCallback<T> *sub_px4 = new SubscriberUORBCallback<T>(uorb_sub, interval, std::bind(fp, std::placeholders::_1)); - /* Check if this is the smallest interval so far and update _sub_min_interval */ - if (_sub_min_interval == nullptr || _sub_min_interval->get_interval() > interval) { - _sub_min_interval = sub_px4; - } - _subs.add((SubscriberNode *)sub_px4); + // [> Check if this is the smallest interval so far and update _sub_min_interval <] + // if (_sub_min_interval == nullptr || _sub_min_interval->get_interval() > interval) { + // _sub_min_interval = sub_px4; + // } + // _subs.add((SubscriberNode *)sub_px4); return (Subscriber<T> *)sub_px4; } diff --git a/src/platforms/px4_publisher.h b/src/platforms/px4_publisher.h index aff045d23..6d75e28fc 100644 --- a/src/platforms/px4_publisher.h +++ b/src/platforms/px4_publisher.h @@ -54,7 +54,7 @@ namespace px4 /** * Untemplated publisher base class * */ -class PublisherBase +class __EXPORT PublisherBase { public: PublisherBase() {}; @@ -99,7 +99,7 @@ private: ros::Publisher _ros_pub; /**< Handle to the ros publisher */ }; #else -class Publisher : +class __EXPORT Publisher : // public uORB::PublicationNode, public PublisherBase, public ListNode<Publisher *> diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h index ef03922ad..88cc86ab8 100644 --- a/src/platforms/px4_subscriber.h +++ b/src/platforms/px4_subscriber.h @@ -57,11 +57,11 @@ namespace px4 /** * Untemplated subscriber base class * */ -class SubscriberBase +class __EXPORT SubscriberBase { public: SubscriberBase() {}; - ~SubscriberBase() {}; + virtual ~SubscriberBase() {}; }; @@ -69,25 +69,25 @@ public: * Subscriber class which is used by nodehandle */ template<typename T> -class Subscriber : +class __EXPORT Subscriber : public SubscriberBase { public: Subscriber() : - SubscriberBase() + SubscriberBase(), + _msg_current() {}; - ~Subscriber() {}; + + virtual ~Subscriber() {} /* Accessors*/ /** * Get the last message value */ - virtual T get() = 0; + virtual T get() {return _msg_current;} - /** - * Get void pointer to last message value - */ - virtual void *get_void_ptr() = 0; +protected: + T _msg_current; /**< Current Message value */ }; #if defined(__PX4_ROS) @@ -104,11 +104,11 @@ public: /** * Construct Subscriber by providing a callback function */ - SubscriberROS(std::function<void(const typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &)> cbf) : + // SubscriberROS(std::function<void(const typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &)> cbf) : + SubscriberROS(std::function<void(const T &)> cbf) : Subscriber<T>(), _ros_sub(), - _cbf(cbf), - _msg_current() + _cbf(cbf) {} /** @@ -117,35 +117,26 @@ public: SubscriberROS() : Subscriber<T>(), _ros_sub(), - _cbf(NULL), - _msg_current() + _cbf(NULL) {} - ~SubscriberROS() {}; - - /* Accessors*/ - /** - * Get the last message value - */ - T get() { return _msg_current; } - /** - * Get void pointer to last message value - */ - void *get_void_ptr() { return (void *)&_msg_current; } + virtual ~SubscriberROS() {}; protected: /** * 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 typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &msg) { /* Store data */ - _msg_current = (T)msg; + this->_msg_current = (T)msg; /* Call callback */ if (_cbf != NULL) { - _cbf(msg); + // _cbf(_msg_current); + _cbf(this->get()); } } @@ -159,8 +150,7 @@ protected: } ros::Subscriber _ros_sub; /**< Handle to ros subscriber */ - std::function<void(const typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &)> _cbf; /**< Callback that the user provided on the subscription */ - T _msg_current; /**< Current Message value */ + std::function<void(const T &)> _cbf; /**< Callback that the user provided on the subscription */ }; @@ -170,7 +160,7 @@ protected: /** * Because we maintain a list of subscribers we need a node class */ -class SubscriberNode : +class __EXPORT SubscriberNode : public ListNode<SubscriberNode *> { public: @@ -197,7 +187,7 @@ protected: * Subscriber class that is templated with the uorb subscription message type */ template<typename T> -class SubscriberUORB : +class __EXPORT SubscriberUORB : public Subscriber<T>, public SubscriberNode { @@ -220,7 +210,7 @@ public: _uorb_sub(uorb_sub) {} - ~SubscriberUORB() {}; + virtual ~SubscriberUORB() {}; /** * Update Subscription @@ -229,11 +219,10 @@ public: virtual void update() { if (!_uorb_sub->updated()) { - /* Topic not updated */ + /* Topic not updated, do not call callback */ return; } - /* get latest data */ _uorb_sub->update(get_void_ptr()); }; @@ -241,11 +230,16 @@ public: /** * Get the last message value */ - T get() { return (T)(typename std::remove_reference<decltype(((T*)nullptr)->data())>::type)*_uorb_sub; } + // T get() { return (T)(typename std::remove_reference<decltype(((T*)nullptr)->data())>::type)*_uorb_sub; } + // T get() { + // typename std::remove_reference<decltype(((T*)nullptr)->data())>::type msg = (typename std::remove_reference<decltype(((T*)nullptr)->data())>::type)*_uorb_sub; + // return (T)msg; + // } + /** * Get void pointer to last message value */ - void *get_void_ptr() { return (void *)(typename std::remove_reference<decltype(((T*)nullptr)->data())>::type*)_uorb_sub; } + void *get_void_ptr() { return (void *)&(this->_msg_current.data()); } int getUORBHandle() { return _uorb_sub->getHandle(); } @@ -256,7 +250,7 @@ protected: //XXX reduce to one class with overloaded constructor? template<typename T> -class SubscriberUORBCallback : +class __EXPORT SubscriberUORBCallback : public SubscriberUORB<T> { public: @@ -275,12 +269,13 @@ public: // _callback(callback) // {} SubscriberUORBCallback(uORB::SubscriptionBase * uorb_sub, - std::function<void(typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &)> callback) : - SubscriberUORB<T>(uorb_sub), - _callback(callback) + unsigned interval, + std::function<void(const T &)> cbf) : + SubscriberUORB<T>(uorb_sub, interval), + _cbf(cbf) {} - ~SubscriberUORBCallback() {}; + virtual ~SubscriberUORBCallback() {}; /** * Update Subscription @@ -289,28 +284,28 @@ public: */ virtual void update() { - if (!SubscriberUORB<T>::_uorb_sub->updated()) { + if (!this->_uorb_sub->updated()) { /* Topic not updated, do not call callback */ return; - } /* get latest data */ - SubscriberUORB<T>::_uorb_sub->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(SubscriberUORB<T>::getUORBData()); + _cbf(Subscriber<T>::get()); + } }; protected: - std::function<void(const typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &)> _callback; /**< Callback handle, - called when new data is available */ + // std::function<void(const typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &)> _callback; [>*< Callback handle, called when new data is available */ + std::function<void(const T &)> _cbf; /**< Callback that the user provided on the subscription */ }; #endif |