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