diff options
Diffstat (limited to 'src/platforms')
-rw-r--r-- | src/platforms/px4_nodehandle.h | 35 | ||||
-rw-r--r-- | src/platforms/px4_publisher.h | 14 | ||||
-rw-r--r-- | src/platforms/px4_subscriber.h | 17 |
3 files changed, 60 insertions, 6 deletions
diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index d66884445..fa2c8d6a4 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -49,21 +49,26 @@ #include <inttypes.h> #else /* includes when building for NuttX */ - +#include <containers/List.hpp> #endif namespace px4 { #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) -class NodeHandle : private ros::NodeHandle +class NodeHandle : + private ros::NodeHandle { public: - NodeHandle () : + NodeHandle() : ros::NodeHandle(), _subs(), _pubs() {} + ~NodeHandle() { + //XXX empty lists + }; + template<typename M> Subscriber subscribe(const char *topic, void(*fp)(M)) { ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, fp); @@ -87,6 +92,30 @@ private: #else class NodeHandle { +public: + NodeHandle() : + _subs(), + _pubs() + {} + + ~NodeHandle() {}; + + template<typename M> + Subscriber subscribe(const char *topic, void(*fp)(M)) { + Subscriber sub(&_subs, , interval); + return sub; + } + + template<typename M> + Publisher advertise(const char *topic) { + Publisher pub(ros_pub); + _pubs.push_back(pub); + return pub; + } +private: + List<Subscriber> _subs; + List<Publisher> _pubs; + }; #endif } diff --git a/src/platforms/px4_publisher.h b/src/platforms/px4_publisher.h index 72f69a5af..53e63b695 100644 --- a/src/platforms/px4_publisher.h +++ b/src/platforms/px4_publisher.h @@ -38,7 +38,11 @@ */ #pragma once #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) +/* includes when building for ros */ #include "ros/ros.h" +#else +/* includes when building for NuttX */ +#include <uORB/Publication.hpp> #endif namespace px4 @@ -56,7 +60,15 @@ private: ros::Publisher _ros_pub; }; #else -class Publisher +template<typename M> +class Publisher : + public uORB::Publication<M> +public: + Publisher(List<SubscriptionBase *> * list, + const struct orb_metadata *meta, unsigned interval) : + uORB::Publication(list, meta) + {} + ~Publisher() {}; { }; #endif diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h index 78be3ebff..6312e0cbe 100644 --- a/src/platforms/px4_subscriber.h +++ b/src/platforms/px4_subscriber.h @@ -38,7 +38,11 @@ */ #pragma once #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) +/* includes when building for ros */ #include "ros/ros.h" +#else +/* includes when building for NuttX */ +#include <uORB/Subscription.hpp> #endif namespace px4 @@ -48,14 +52,23 @@ namespace px4 class Subscriber { public: - Subscriber(ros::Subscriber ros_sub) : _ros_sub(ros_sub) + Subscriber(ros::Subscriber ros_sub) : + _ros_sub(ros_sub) {} ~Subscriber() {}; private: ros::Subscriber _ros_sub; }; #else -class Subscriber +template<typename M> +class Subscriber : + public uORB::Subscription<M> +public: + Subscriber(List<SubscriptionBase *> * list, + const struct orb_metadata *meta, unsigned interval) : + uORB::Subsciption(list, meta, interval) + {} + ~Subscriber() {}; { }; #endif |