From cadcad6ffbdfbe9b92a5936f4d894138f912b4ff Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 20 Jan 2015 18:27:05 +0100 Subject: messagelayer prototype for nuttx --- src/platforms/nuttx/px4_messages/px4_rc_channels.h | 2 +- src/platforms/px4_message.h | 3 +- src/platforms/px4_nodehandle.h | 20 +++-- src/platforms/px4_publisher.h | 4 +- src/platforms/px4_subscriber.h | 93 ++++++++++------------ 5 files changed, 61 insertions(+), 61 deletions(-) (limited to 'src/platforms') 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 { 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 -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 *)sub; // } template - Subscriber *subscribe(void(*fp)(const typename std::remove_referencedata())>::type &)) + // Subscriber *subscribe(void(*fp)(const typename std::remove_referencedata())>::type &)) + Subscriber *subscribe(void(*fp)(const T &)) { SubscriberBase *sub = new SubscriberROS(std::bind(fp, std::placeholders::_1)); ros::Subscriber ros_sub = ros::NodeHandle::subscribe((new T())->handle(), kQueueSizeDefault, @@ -240,17 +241,20 @@ public: */ template - Subscriber *subscribe(std::functiondata())>::type &)> callback, unsigned interval=10) //XXX interval + // Subscriber *subscribe(std::functiondata())>::type &)> callback, unsigned interval=10) //XXX interval + // Subscriber *subscribe(void(*fp)(const typename std::remove_referencedata())>::type &), unsigned interval=10) //XXX interval + Subscriber *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 *sub_px4 = new SubscriberUORBCallback(uorb_sub, callback); + // SubscriberUORBCallback *sub_px4 = new SubscriberUORBCallback(uorb_sub, interval, callback); + SubscriberUORBCallback *sub_px4 = new SubscriberUORBCallback(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 *)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 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 -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::functiondata())>::type &)> cbf) : + // SubscriberROS(std::functiondata())>::type &)> cbf) : + SubscriberROS(std::function cbf) : Subscriber(), _ros_sub(), - _cbf(cbf), - _msg_current() + _cbf(cbf) {} /** @@ -117,35 +117,26 @@ public: SubscriberROS() : Subscriber(), _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_referencedata())>::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::functiondata())>::type &)> _cbf; /**< Callback that the user provided on the subscription */ - T _msg_current; /**< Current Message value */ + std::function _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 { public: @@ -197,7 +187,7 @@ protected: * Subscriber class that is templated with the uorb subscription message type */ template -class SubscriberUORB : +class __EXPORT SubscriberUORB : public Subscriber, 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_referencedata())>::type)*_uorb_sub; } + // T get() { return (T)(typename std::remove_referencedata())>::type)*_uorb_sub; } + // T get() { + // typename std::remove_referencedata())>::type msg = (typename std::remove_referencedata())>::type)*_uorb_sub; + // return (T)msg; + // } + /** * Get void pointer to last message value */ - void *get_void_ptr() { return (void *)(typename std::remove_referencedata())>::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 -class SubscriberUORBCallback : +class __EXPORT SubscriberUORBCallback : public SubscriberUORB { public: @@ -275,12 +269,13 @@ public: // _callback(callback) // {} SubscriberUORBCallback(uORB::SubscriptionBase * uorb_sub, - std::functiondata())>::type &)> callback) : - SubscriberUORB(uorb_sub), - _callback(callback) + unsigned interval, + std::function cbf) : + SubscriberUORB(uorb_sub, interval), + _cbf(cbf) {} - ~SubscriberUORBCallback() {}; + virtual ~SubscriberUORBCallback() {}; /** * Update Subscription @@ -289,28 +284,28 @@ public: */ virtual void update() { - if (!SubscriberUORB::_uorb_sub->updated()) { + if (!this->_uorb_sub->updated()) { /* Topic not updated, do not call callback */ return; - } /* get latest data */ - SubscriberUORB::_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::getUORBData()); + _cbf(Subscriber::get()); + } }; protected: - std::functiondata())>::type &)> _callback; /**< Callback handle, - called when new data is available */ + // std::functiondata())>::type &)> _callback; [>*< Callback handle, called when new data is available */ + std::function _cbf; /**< Callback that the user provided on the subscription */ }; #endif -- cgit v1.2.3