diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2014-12-11 15:04:37 +0100 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-12-11 15:04:37 +0100 |
commit | c68c277c94cacd2a64b634dd9c45ace2a04c2911 (patch) | |
tree | 20e875d0a0f3c51c69b98f6a82aaefdedf8621bf /src/platforms/px4_nodehandle.h | |
parent | fd6a5fd38b194300788c597fd1636b2a97e24642 (diff) | |
download | px4-firmware-c68c277c94cacd2a64b634dd9c45ace2a04c2911.tar.gz px4-firmware-c68c277c94cacd2a64b634dd9c45ace2a04c2911.tar.bz2 px4-firmware-c68c277c94cacd2a64b634dd9c45ace2a04c2911.zip |
subscription class for ros now stores last message to avoid manual copy and support subscription with no callbacks
Diffstat (limited to 'src/platforms/px4_nodehandle.h')
-rw-r--r-- | src/platforms/px4_nodehandle.h | 25 |
1 files changed, 14 insertions, 11 deletions
diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index 25b8e037d..c24a18303 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -77,10 +77,11 @@ public: * @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)(const M&)) { - ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, fp); - Subscriber *sub = new Subscriber(ros_sub); + Subscriber *sub = new SubscriberROS<M>(std::bind(fp, std::placeholders::_1)); + ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS<M>::callback, (SubscriberROS<M>*)sub); + ((SubscriberROS<M>*)sub)->set_ros_sub(ros_sub); _subs.push_back(sub); return sub; } @@ -91,10 +92,11 @@ public: * @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)(const M&), T *obj) { - ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, fp, obj); - Subscriber *sub = new Subscriber(ros_sub); + Subscriber *sub = new SubscriberROS<M>(std::bind(fp, obj, std::placeholders::_1)); + ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS<M>::callback, (SubscriberROS<M>*)sub); + ((SubscriberROS<M>*)sub)->set_ros_sub(ros_sub); _subs.push_back(sub); return sub; } @@ -106,10 +108,11 @@ public: template<typename M> Subscriber *subscribe(const char *topic) { - //XXX missing implementation - // Subscriber *sub = new Subscriber(ros_sub); - // _subs.push_back(sub); - return (Subscriber *)NULL; + Subscriber *sub = new SubscriberROS<M>(); + ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS<M>::callback, (SubscriberROS<M>*)sub); + ((SubscriberROS<M>*)sub)->set_ros_sub(ros_sub); + _subs.push_back(sub); + return sub; } /** @@ -165,7 +168,7 @@ public: std::function<void(const M &)> callback, unsigned interval) { - SubscriberPX4<M> *sub_px4 = new SubscriberPX4<M>(meta, interval, callback, &_subs); + SubscriberUORB<M> *sub_px4 = new SubscriberUORB<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()) { |