diff options
| -rw-r--r-- | src/platforms/px4_nodehandle.h | 15 | ||||
| -rw-r--r-- | src/platforms/px4_subscriber.h | 29 |
2 files changed, 13 insertions, 31 deletions
diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index d78c865de..40604aa86 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -81,9 +81,7 @@ public: template<typename T> 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(T::handle(), kQueueSizeDefault, - &SubscriberROS<T>::callback, (SubscriberROS<T> *)sub); - ((SubscriberROS<T> *)sub)->set_ros_sub(ros_sub); + SubscriberBase *sub = new SubscriberROS<T>((ros::NodeHandle*)this, std::bind(fp, std::placeholders::_1)); _subs.push_back(sub); return (Subscriber<T> *)sub; } @@ -96,11 +94,7 @@ public: template<typename T, typename C> Subscriber<T> *subscribe(void(C::*fp)(const T &), C *obj) { - SubscriberBase *sub = new SubscriberROS<T>(std::bind(fp, obj, std::placeholders::_1)); - ros::Subscriber ros_sub = ros::NodeHandle::subscribe(T::handle(), kQueueSizeDefault, - &SubscriberROS<T>::callback, (SubscriberROS<T> *)sub);//XXX needs cleanup in destructor ore move into class - - ((SubscriberROS<T> *)sub)->set_ros_sub(ros_sub); + SubscriberBase *sub = new SubscriberROS<T>((ros::NodeHandle*)this, std::bind(fp, obj, std::placeholders::_1)); _subs.push_back(sub); return (Subscriber<T> *)sub; } @@ -111,10 +105,7 @@ public: template<typename T> Subscriber<T> *subscribe() { - SubscriberBase *sub = new SubscriberROS<T>(); - ros::Subscriber ros_sub = ros::NodeHandle::subscribe(T::handle(), kQueueSizeDefault, - &SubscriberROS<T>::callback, (SubscriberROS<T> *)sub); - ((SubscriberROS<T> *)sub)->set_ros_sub(ros_sub); + SubscriberBase *sub = new SubscriberROS<T>((ros::NodeHandle*)this); _subs.push_back(sub); return (Subscriber<T> *)sub; } diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h index c499712a9..a54b8eb08 100644 --- a/src/platforms/px4_subscriber.h +++ b/src/platforms/px4_subscriber.h @@ -102,24 +102,23 @@ class SubscriberROS : public: /** - * Construct Subscriber by providing a callback function + * Construct Subscriber without a callback function */ - SubscriberROS(std::function<void(const T &)> cbf) : - Subscriber<T>(), - _ros_sub(), - _cbf(cbf) + SubscriberROS(ros::NodeHandle *rnh) : + px4::Subscriber<T>(), + _cbf(NULL), + _ros_sub(rnh->subscribe(T::handle(), 1, &SubscriberROS<T>::callback, this)) {} /** - * Construct Subscriber without a callback function + * Construct Subscriber by providing a callback function */ - SubscriberROS() : - Subscriber<T>(), - _ros_sub(), - _cbf(NULL) + //XXX queue default + SubscriberROS(ros::NodeHandle *rnh, std::function<void(const T &)> cbf) : + _cbf(cbf), + _ros_sub(rnh->subscribe(T::handle(), 1, &SubscriberROS<T>::callback, this)) {} - virtual ~SubscriberROS() {}; protected: @@ -139,14 +138,6 @@ protected: } - /** - * 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 T &)> _cbf; /**< Callback that the user provided on the subscription */ |