diff options
-rw-r--r-- | src/examples/subscriber/subscriber.cpp | 2 | ||||
-rw-r--r-- | src/include/containers/List.hpp | 1 | ||||
-rw-r--r-- | src/platforms/px4_nodehandle.h | 14 |
3 files changed, 14 insertions, 3 deletions
diff --git a/src/examples/subscriber/subscriber.cpp b/src/examples/subscriber/subscriber.cpp index 5d1d14d7f..828b1f937 100644 --- a/src/examples/subscriber/subscriber.cpp +++ b/src/examples/subscriber/subscriber.cpp @@ -74,7 +74,7 @@ int main(int argc, char **argv) * is the number of messages that will be buffered up before beginning to throw * away the oldest ones. */ - px4::Subscriber* sub = n.subscribe("rc_channels", rc_channels_callback); + px4::Subscriber sub = n.subscribe("rc_channels", rc_channels_callback); PX4_INFO("subscribed"); /** diff --git a/src/include/containers/List.hpp b/src/include/containers/List.hpp index 13cbda938..5c0ba59fd 100644 --- a/src/include/containers/List.hpp +++ b/src/include/containers/List.hpp @@ -38,6 +38,7 @@ */ #pragma once +#include <cstddef> template<class T> class __EXPORT ListNode diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index 809d0eb15..9bbf5c724 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -40,6 +40,7 @@ #include <px4_subscriber.h> #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) #include "ros/ros.h" +#include <list> #endif namespace px4 @@ -48,12 +49,21 @@ namespace px4 class NodeHandle : private ros::NodeHandle { public: + NodeHandle () : + ros::NodeHandle(), + _subs() + {} + template<class M> - Subscriber* subscribe(const char *topic, void(*fp)(M)) { + Subscriber subscribe(const char *topic, void(*fp)(M)) { ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, 1000, fp); //XXX create list here, for ros and nuttx - return new Subscriber(ros_sub); + Subscriber sub(ros_sub); + _subs.push_back(sub); + return sub; } +private: + std::list<Subscriber> _subs; }; #else class NodeHandle |