added a messageplayer prototype for ros

11 files changed, 388 insertions, 116 deletions

diff --git a/src/examples/publisher/publisher_example.cpp b/src/examples/publisher/publisher_example.cpp
index 2e5779ebe..e85e42b38 100644
--- a/src/examples/publisher/publisher_example.cpp
+++ b/src/examples/publisher/publisher_example.cpp
@@ -45,7 +45,8 @@ using namespace px4;
 
 PublisherExample::PublisherExample() :
 	_n(),
-	_rc_channels_pub(PX4_ADVERTISE(_n, rc_channels))
+	_rc_channels_pub(_n.advertise<px4_rc_channels>())
+	// _rc_channels_pub(PX4_ADVERTISE(_n, rc_channels))
 {
 
 }
@@ -55,11 +56,19 @@ int PublisherExample::main()
 	px4::Rate loop_rate(10);
 
 	while (px4::ok()) {
-		PX4_TOPIC_T(rc_channels) msg;
-		msg.timestamp_last_valid = px4::get_time_micros();
-		PX4_INFO("%llu", msg.timestamp_last_valid);
+		// PX4_TOPIC_T(rc_channels) msg;
+		// msg.timestamp_last_valid = px4::get_time_micros();
+		// PX4_INFO("%llu", msg.timestamp_last_valid);
+
+		// _rc_channels_pub->publish(msg);
+
+		//XXX
+		px4_rc_channels msg2;
+		msg2.data().timestamp_last_valid = px4::get_time_micros();
+		PX4_INFO("%llu", msg2.data().timestamp_last_valid);
+		// msg2.pub->publish2();
+		_rc_channels_pub->publish(msg2);
 
-		_rc_channels_pub->publish(msg);
 
 		_n.spinOnce();
 		loop_rate.sleep();
diff --git a/src/examples/publisher/publisher_example.h b/src/examples/publisher/publisher_example.h
index 304ecef47..86586d0d3 100644
--- a/src/examples/publisher/publisher_example.h
+++ b/src/examples/publisher/publisher_example.h
@@ -48,5 +48,6 @@ public:
 	int main();
 protected:
 	px4::NodeHandle _n;
+	// px4::Publisher<px4::px4_rc_channels> * _rc_channels_pub;
 	px4::Publisher * _rc_channels_pub;
 };
diff --git a/src/examples/subscriber/subscriber_example.cpp b/src/examples/subscriber/subscriber_example.cpp
index 3c80561ca..1c8f4c62b 100644
--- a/src/examples/subscriber/subscriber_example.cpp
+++ b/src/examples/subscriber/subscriber_example.cpp
@@ -62,11 +62,13 @@ SubscriberExample::SubscriberExample() :
 
 	/* Do some subscriptions */
 	/* Function */
-	PX4_SUBSCRIBE(_n, rc_channels, rc_channels_callback_function, _interval);
-	/* Class Method */
-	PX4_SUBSCRIBE(_n, rc_channels, SubscriberExample::rc_channels_callback, this, 1000);
-	/* No callback */
-	_sub_rc_chan = PX4_SUBSCRIBE(_n, rc_channels, 500);
+	// PX4_SUBSCRIBE(_n, rc_channels, rc_channels_callback_function, _interval);
+	_n.subscribe<px4_rc_channels>(rc_channels_callback_function);
+
+	// [> Class Method <]
+	// PX4_SUBSCRIBE(_n, rc_channels, SubscriberExample::rc_channels_callback, this, 1000);
+	// [> No callback <]
+	// _sub_rc_chan = PX4_SUBSCRIBE(_n, rc_channels, 500);
 
 	PX4_INFO("subscribed");
 }
@@ -78,5 +80,5 @@ SubscriberExample::SubscriberExample() :
 void SubscriberExample::rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg) {
 	PX4_INFO("Subscriber callback: [%llu], value of _sub_rc_chan: [%llu]",
 			msg.timestamp_last_valid,
-			_sub_rc_chan->get().timestamp_last_valid);
+			_sub_rc_chan->get().data().timestamp_last_valid);
 }
diff --git a/src/examples/subscriber/subscriber_example.h b/src/examples/subscriber/subscriber_example.h
index eb2c956e0..b52ae991b 100644
--- a/src/examples/subscriber/subscriber_example.h
+++ b/src/examples/subscriber/subscriber_example.h
@@ -56,7 +56,8 @@ protected:
 	int32_t _interval;
 	px4_param_t _p_test_float;
 	float _test_float;
-	px4::PX4_SUBSCRIBER(rc_channels) * _sub_rc_chan;
+	// px4::PX4_SUBSCRIBER(rc_channels) * _sub_rc_chan;
+	px4::Subscriber<px4_rc_channels> * _sub_rc_chan;
 
 	void rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg);
 
diff --git a/src/platforms/nuttx/px4_messages/px4_rc_channels.h b/src/platforms/nuttx/px4_messages/px4_rc_channels.h
new file mode 100644
index 000000000..bfca48469
--- /dev/null
+++ b/src/platforms/nuttx/px4_messages/px4_rc_channels.h
@@ -0,0 +1,26 @@
+#include <uORB/uORB.h>
+#include <uORB/topics/rc_channels.h>
+#include "platforms/px4_message.h"
+
+#pragma once
+namespace px4
+{
+
+class  px4_rc_channels :
+	public PX4Message<rc_channels_s>
+{
+public:
+	px4_rc_channels() :
+		PX4Message<rc_channels_s>()
+	{}
+
+	px4_rc_channels(rc_channels_s msg) :
+		PX4Message<rc_channels_s>(msg)
+	{}
+
+	~px4_rc_channels() {}
+
+	PX4TopicHandle handle() {return (PX4TopicHandle)ORB_ID(rc_channels);}
+};
+
+}
diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h
index fc31162c8..bdb2b8de9 100644
--- a/src/platforms/px4_includes.h
+++ b/src/platforms/px4_includes.h
@@ -49,6 +49,7 @@
 #ifdef __cplusplus
 #include "ros/ros.h"
 #include "px4/rc_channels.h"
+#include "px4_rc_channels.h" //XXX
 #include "px4/vehicle_attitude.h"
 #include <px4/vehicle_attitude_setpoint.h>
 #include <px4/manual_control_setpoint.h>
@@ -71,6 +72,9 @@
 #include <nuttx/config.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/rc_channels.h>
+#ifdef __cplusplus
+#include <platforms/nuttx/px4_messages/px4_rc_channels.h> //XXX
+#endif
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/actuator_controls.h>
diff --git a/src/platforms/px4_message.h b/src/platforms/px4_message.h
new file mode 100644
index 000000000..05fcf1140
--- /dev/null
+++ b/src/platforms/px4_message.h
@@ -0,0 +1,77 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file px4_message.h
+ *
+ * Defines the message base types
+ */
+#pragma once
+
+#if defined(__PX4_ROS)
+typedef const char* PX4TopicHandle;
+#else
+#include <uORB/uORB.h>
+typedef const struct orb_metatdata* PX4TopicHandle;
+#endif
+
+namespace px4
+{
+
+template <typename M>
+class PX4Message
+{
+	// friend class NodeHandle;
+// #if defined(__PX4_ROS)
+	// template<typename T>
+	// friend class SubscriberROS;
+// #endif
+
+public:
+	PX4Message() :
+		_data()
+	{}
+
+	PX4Message(M msg) :
+		_data(msg)
+	{}
+
+	virtual ~PX4Message() {};
+
+	virtual M& data() {return _data;}
+	virtual PX4TopicHandle handle() = 0;
+private:
+	M _data;
+};
+
+}
diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h
index 7b14caed9..a40581239 100644
--- a/src/platforms/px4_nodehandle.h
+++ b/src/platforms/px4_nodehandle.h
@@ -48,6 +48,7 @@
 #include "ros/ros.h"
 #include <list>
 #include <inttypes.h>
+#include <type_traits>
 #else
 /* includes when building for NuttX */
 #include <poll.h>
@@ -77,15 +78,25 @@ public:
 	 * @param topic		Name of the topic
 	 * @param fb		Callback, executed on receiving a new message
 	 */
-	template<typename M>
-	Subscriber<M> *subscribe(const char *topic, void(*fp)(const M &))
+	// template<typename M>
+	// Subscriber<M> *subscribe(const char *topic, void(*fp)(const M &))
+	// {
+		// SubscriberBase *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 (Subscriber<M> *)sub;
+	// }
+	template<typename T>
+	Subscriber<T> *subscribe(void(*fp)(const typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &))
 	{
-		SubscriberBase *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);
+		SubscriberBase *sub = new SubscriberROS<T>(std::bind(fp, std::placeholders::_1));
+		ros::Subscriber ros_sub = ros::NodeHandle::subscribe((new T())->handle(), kQueueSizeDefault,
+				&SubscriberROS<T>::callback, (SubscriberROS<T> *)sub);
+		((SubscriberROS<T> *)sub)->set_ros_sub(ros_sub);
 		_subs.push_back(sub);
-		return (Subscriber<M> *)sub;
+		return (Subscriber<T> *)sub;
 	}
 
 	/**
@@ -93,40 +104,49 @@ public:
 	 * @param topic		Name of the topic
 	 * @param fb		Callback, executed on receiving a new message
 	 */
-	template<typename M, typename T>
-	Subscriber<M> *subscribe(const char *topic, void(T::*fp)(const M &), T *obj)
-	{
-		SubscriberBase *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 (Subscriber<M> *)sub;
-	}
+	// template<typename M, typename T>
+	// Subscriber<M> *subscribe(const char *topic, void(T::*fp)(const M &), T *obj)
+	// {
+		// SubscriberBase *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 (Subscriber<M> *)sub;
+	// }
 
 	/**
 	 * Subscribe with no callback, just the latest value is stored on updates
 	 * @param topic		Name of the topic
 	 */
-	template<typename M>
-	Subscriber<M> *subscribe(const char *topic)
-	{
-		SubscriberBase *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 (Subscriber<M> *)sub;
-	}
+	// template<typename M>
+	// Subscriber<M> *subscribe(const char *topic)
+	// {
+		// SubscriberBase *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 (Subscriber<M> *)sub;
+	// }
 
 	/**
 	 * Advertise topic
 	 * @param topic		Name of the topic
 	 */
-	template<typename M>
-	Publisher *advertise(const char *topic)
+	// template<typename M>
+	// Publisher *advertise(const char *topic)
+	// {
+		// ros::Publisher ros_pub = ros::NodeHandle::advertise<M>(topic, kQueueSizeDefault);
+		// Publisher *pub =  new Publisher(ros_pub);
+		// _pubs.push_back(pub);
+		// return pub;
+	// }
+	template<typename T>
+	Publisher* advertise()
+	// Publisher<T> *advertise()
 	{
-		ros::Publisher ros_pub = ros::NodeHandle::advertise<M>(topic, kQueueSizeDefault);
+		ros::Publisher ros_pub = ros::NodeHandle::advertise<typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &>((new T())->handle(), kQueueSizeDefault);
 		Publisher *pub =  new Publisher(ros_pub);
 		_pubs.push_back(pub);
 		return pub;
@@ -161,7 +181,7 @@ public:
 	~NodeHandle()
 	{
 		/* Empty subscriptions list */
-		uORB::SubscriptionNode *sub = _subs.getHead();
+		SubscriberNode *sub = _subs.getHead();
 		int count = 0;
 
 		while (sub != nullptr) {
@@ -170,13 +190,13 @@ public:
 				break;
 			}
 
-			uORB::SubscriptionNode *sib = sub->getSibling();
+			SubscriberNode *sib = sub->getSibling();
 			delete sub;
 			sub = sib;
 		}
 
 		/* Empty publications list */
-		uORB::PublicationNode *pub = _pubs.getHead();
+		Publisher *pub = _pubs.getHead();
 		count = 0;
 
 		while (pub != nullptr) {
@@ -185,7 +205,7 @@ public:
 				break;
 			}
 
-			uORB::PublicationNode *sib = pub->getSibling();
+			Publisher *sib = pub->getSibling();
 			delete pub;
 			pub = sib;
 		}
@@ -198,19 +218,41 @@ public:
 	 * @param interval	Minimal interval between calls to callback
 	 */
 
-	template<typename M>
-	Subscriber<M> *subscribe(const struct orb_metadata *meta,
-				 std::function<void(const M &)> callback,
-				 unsigned interval)
+	// template<typename M>
+	// Subscriber<M> *subscribe(const struct orb_metadata *meta,
+				 // std::function<void(const M &)> callback,
+				 // unsigned interval)
+	// {
+		// SubscriberUORBCallback<M> *sub_px4 = new SubscriberUORBCallback<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()) {
+			// _sub_min_interval = sub_px4;
+		// }
+
+		// return (Subscriber<M> *)sub_px4;
+	// }
+	/**
+	 * Subscribe with callback to function
+	 * @param meta		Describes the topic which nodehande should subscribe to
+	 * @param callback	Callback, executed on receiving a new message
+	 * @param interval	Minimal interval between calls to callback
+	 */
+
+	template<typename T>
+	Subscriber<T> *subscribe(std::function<void(const typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &)> callback,  unsigned interval=10) //XXX interval
 	{
-		SubscriberUORBCallback<M> *sub_px4 = new SubscriberUORBCallback<M>(meta, interval, callback, &_subs);
+		const struct orb_metadata * meta = NULL;
+		uORB::SubscriptionBase * uorb_sub = new uORB::SubscriptionBase(meta, interval);
+		SubscriberUORBCallback<T> *sub_px4 = new SubscriberUORBCallback<T>(uorb_sub, callback);
 
 		/* 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()) {
+		if (_sub_min_interval == nullptr || _sub_min_interval->get_interval() > interval) {
 			_sub_min_interval = sub_px4;
 		}
+		_subs.add((SubscriberNode *)sub_px4);
 
-		return (Subscriber<M> *)sub_px4;
+		return (Subscriber<T> *)sub_px4;
 	}
 
 	/**
@@ -219,29 +261,47 @@ public:
 	 * @param interval	Minimal interval between data fetches from orb
 	 */
 
-	template<typename M>
-	Subscriber<M> *subscribe(const struct orb_metadata *meta,
-				 unsigned interval)
-	{
-		SubscriberUORB<M> *sub_px4 = new SubscriberUORB<M>(meta, interval, &_subs);
+	// template<typename M>
+	// Subscriber<M> *subscribe(const struct orb_metadata *meta,
+				 // unsigned interval)
+	// {
+		// SubscriberUORB<M> *sub_px4 = new SubscriberUORB<M>(meta, interval, &_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()) {
-			_sub_min_interval = sub_px4;
-		}
+		// [> 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()) {
+			// _sub_min_interval = sub_px4;
+		// }
 
-		return (Subscriber<M> *)sub_px4;
-	}
+		// return (Subscriber<M> *)sub_px4;
+	// }
 
 	/**
 	 * Advertise topic
 	 * @param meta		Describes the topic which is advertised
 	 */
-	template<typename M>
-	Publisher *advertise(const struct orb_metadata *meta)
+	// template<typename M>
+	// Publisher *advertise(const struct orb_metadata *meta)
+	// {
+		// //XXX
+		// Publisher *pub = new Publisher(meta, &_pubs);
+		// return pub;
+	// }
+
+	/**
+	 * Advertise topic
+	 * @param meta		Describes the topic which is advertised
+	 */
+	template<typename T>
+	Publisher *advertise()
 	{
 		//XXX
-		Publisher *pub = new Publisher(meta, &_pubs);
+		// uORB::PublicationBase * uorb_pub = new uORB::PublicationBase((new T())->handle());
+		const struct orb_metadata * meta = NULL;
+		uORB::PublicationBase * uorb_pub = new uORB::PublicationBase(meta);
+		Publisher *pub = new Publisher(uorb_pub);
+
+		_pubs.add(pub);
+
 		return pub;
 	}
 
@@ -251,7 +311,7 @@ public:
 	void spinOnce()
 	{
 		/* Loop through subscriptions, call callback for updated subscriptions */
-		uORB::SubscriptionNode *sub = _subs.getHead();
+		SubscriberNode *sub = _subs.getHead();
 		int count = 0;
 
 		while (sub != nullptr) {
@@ -281,7 +341,7 @@ public:
 
 			/* Poll fd with smallest interval */
 			struct pollfd pfd;
-			pfd.fd = _sub_min_interval->getHandle();
+			pfd.fd = _sub_min_interval->getUORBHandle();
 			pfd.events = POLLIN;
 			poll(&pfd, 1, timeout_ms);
 			spinOnce();
@@ -290,9 +350,9 @@ public:
 private:
 	static const uint16_t kMaxSubscriptions = 100;
 	static const uint16_t kMaxPublications = 100;
-	List<uORB::SubscriptionNode *> _subs;		/**< Subcriptions of node */
-	List<uORB::PublicationNode *> _pubs;		/**< Publications of node */
-	uORB::SubscriptionNode *_sub_min_interval;	/**< Points to the sub wtih the smallest interval
+	List<SubscriberNode *> _subs;		/**< Subcriptions of node */
+	List<Publisher *> _pubs;		/**< Publications of node */
+	SubscriberNode *_sub_min_interval;	/**< Points to the sub wtih the smallest interval
 							  of all Subscriptions in _subs*/
 };
 #endif
diff --git a/src/platforms/px4_publisher.h b/src/platforms/px4_publisher.h
index c6f3d6108..aff045d23 100644
--- a/src/platforms/px4_publisher.h
+++ b/src/platforms/px4_publisher.h
@@ -46,6 +46,8 @@
 #include <containers/List.hpp>
 #endif
 
+#include <platforms/px4_message.h>
+
 namespace px4
 {
 
@@ -58,11 +60,19 @@ public:
 	PublisherBase() {};
 	~PublisherBase() {};
 
+	/** Publishes msg
+	 * @param msg	    the message which is published to the topic
+	 */
+	// virtual int publish2(const PX4Message * const msg) = 0;
+
 };
 
 #if defined(__PX4_ROS)
+// template <typename T>
 class Publisher :
 	public PublisherBase
+	// public PublisherBase,
+	// public T
 {
 public:
 	/**
@@ -71,6 +81,7 @@ public:
 	 */
 	Publisher(ros::Publisher ros_pub) :
 		PublisherBase(),
+		// T(),
 		_ros_pub(ros_pub)
 	{}
 
@@ -79,15 +90,20 @@ public:
 	/** Publishes msg
 	 * @param msg	    the message which is published to the topic
 	 */
-	template<typename M>
-	int publish(const M &msg) { _ros_pub.publish(msg); return 0; }
+	// int publish(const M &msg) { _ros_pub.publish(msg); return 0; }
+	template <typename T>
+	int publish(T &msg) {
+		_ros_pub.publish(msg.data());
+		return 0;}
 private:
 	ros::Publisher _ros_pub;	/**< Handle to the ros publisher */
 };
 #else
 class Publisher :
-	public uORB::PublicationNode,
-	public PublisherBase
+	// public uORB::PublicationNode,
+	public PublisherBase,
+	public ListNode<Publisher *>
+
 {
 public:
 	/**
@@ -95,10 +111,14 @@ public:
 	 * @param meta	    orb metadata for the topic which is used
 	 * @param list	    publisher is added to this list
 	 */
-	Publisher(const struct orb_metadata *meta,
-		  List<uORB::PublicationNode *> *list) :
-		uORB::PublicationNode(meta, list),
-		PublisherBase()
+	// Publisher(const struct orb_metadata *meta,
+		  // List<uORB::PublicationNode *> *list) :
+		// uORB::PublicationNode(meta, list),
+		// PublisherBase()
+	// {}
+	Publisher(uORB::PublicationBase * uorb_pub) :
+		PublisherBase(),
+		_uorb_pub(uorb_pub)
 	{}
 
 	~Publisher() {};
@@ -109,7 +129,7 @@ public:
 	template<typename M>
 	int publish(const M &msg)
 	{
-		uORB::PublicationBase::update((void *)&msg);
+		_uorb_pub->update((void *)&msg);
 		return 0;
 	}
 
@@ -117,6 +137,9 @@ public:
 	 * Empty callback for list traversal
 	 */
 	void update() {} ;
+private:
+	uORB::PublicationBase * _uorb_pub;	/**< Handle to the publisher */
+
 };
 #endif
 }
diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h
index aaacf9e48..ef03922ad 100644
--- a/src/platforms/px4_subscriber.h
+++ b/src/platforms/px4_subscriber.h
@@ -39,6 +39,7 @@
 #pragma once
 
 #include <functional>
+#include <type_traits>
 
 #if defined(__PX4_ROS)
 /* includes when building for ros */
@@ -67,7 +68,7 @@ public:
 /**
  * Subscriber class which is used by nodehandle
  */
-template<typename M>
+template<typename T>
 class Subscriber :
 	public SubscriberBase
 {
@@ -81,7 +82,7 @@ public:
 	/**
 	 * Get the last message value
 	 */
-	virtual M get() = 0;
+	virtual T get() = 0;
 
 	/**
 	 * Get void pointer to last message value
@@ -93,9 +94,9 @@ public:
 /**
  * Subscriber class that is templated with the ros n message type
  */
-template<typename M>
+template<typename T>
 class SubscriberROS :
-	public Subscriber<M>
+	public Subscriber<T>
 {
 	friend class NodeHandle;
 
@@ -103,8 +104,8 @@ public:
 	/**
 	 * Construct Subscriber by providing a callback function
 	 */
-	SubscriberROS(std::function<void(const M &)> cbf) :
-		Subscriber<M>(),
+	SubscriberROS(std::function<void(const typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &)> cbf) :
+		Subscriber<T>(),
 		_ros_sub(),
 		_cbf(cbf),
 		_msg_current()
@@ -114,7 +115,7 @@ public:
 	 * Construct Subscriber without a callback function
 	 */
 	SubscriberROS() :
-		Subscriber<M>(),
+		Subscriber<T>(),
 		_ros_sub(),
 		_cbf(NULL),
 		_msg_current()
@@ -127,7 +128,7 @@ public:
 	/**
 	 * Get the last message value
 	 */
-	M get() { return _msg_current; }
+	T get() { return _msg_current; }
 	/**
 	 * Get void pointer to last message value
 	 */
@@ -137,10 +138,10 @@ protected:
 	/**
 	 * Called on topic update, saves the current message and then calls the provided callback function
 	 */
-	void callback(const M &msg)
+	void callback(const typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &msg)
 	{
 		/* Store data */
-		_msg_current = msg;
+		_msg_current = (T)msg;
 
 		/* Call callback */
 		if (_cbf != NULL) {
@@ -158,20 +159,47 @@ protected:
 	}
 
 	ros::Subscriber _ros_sub;		/**< Handle to ros subscriber */
-	std::function<void(const M &)> _cbf;	/**< Callback that the user provided on the subscription */
-	M _msg_current;				/**< Current Message value */
+	std::function<void(const typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &)> _cbf;	/**< Callback that the user provided on the subscription */
+	T _msg_current;				/**< Current Message value */
 
 };
 
 #else // Building for NuttX
 
+
+/**
+ * Because we maintain a list of subscribers we need a node class
+ */
+class SubscriberNode :
+	public ListNode<SubscriberNode *>
+{
+public:
+	SubscriberNode(unsigned interval) :
+		ListNode(),
+		_interval(interval)
+	{}
+
+	virtual ~SubscriberNode() {}
+
+	virtual void update() = 0;
+
+	virtual int getUORBHandle() = 0;
+
+	unsigned get_interval() { return _interval; }
+
+protected:
+	unsigned _interval;
+
+};
+
+
 /**
  * Subscriber class that is templated with the uorb subscription message type
  */
-template<typename M>
+template<typename T>
 class SubscriberUORB :
-	public Subscriber<M>,
-	public uORB::Subscription<M>
+	public Subscriber<T>,
+	public SubscriberNode
 {
 public:
 
@@ -181,11 +209,15 @@ public:
 	 * @param interval	Minimal interval between calls to callback
 	 * @param list	    subscriber is added to this list
 	 */
-	SubscriberUORB(const struct orb_metadata *meta,
-		       unsigned interval,
-		       List<uORB::SubscriptionNode *> *list) :
-		Subscriber<M>(),
-		uORB::Subscription<M>(meta, interval, list)
+	// SubscriberUORB(const struct orb_metadata *meta,
+		       // unsigned interval,
+		       // List<uORB::SubscriptionNode *> *list) :
+		// Subscriber<M>(),
+		// uORB::Subscription<M>(meta, interval, list)
+	// {}
+	SubscriberUORB(uORB::SubscriptionBase * uorb_sub, unsigned interval) :
+		SubscriberNode(interval),
+		_uorb_sub(uorb_sub)
 	{}
 
 	~SubscriberUORB() {};
@@ -196,29 +228,36 @@ public:
 	 */
 	virtual void update()
 	{
-		if (!uORB::Subscription<M>::updated()) {
+		if (!_uorb_sub->updated()) {
 			/* Topic not updated */
 			return;
 		}
 
 		/* get latest data */
-		uORB::Subscription<M>::update();
+		_uorb_sub->update(get_void_ptr());
 	};
 
 	/* Accessors*/
 	/**
 	 * Get the last message value
 	 */
-	M get() { return uORB::Subscription<M>::getData(); }
+	T get() { return (T)(typename std::remove_reference<decltype(((T*)nullptr)->data())>::type)*_uorb_sub; }
 	/**
 	 * Get void pointer to last message value
 	 */
-	void *get_void_ptr() { return uORB::Subscription<M>::getDataVoidPtr(); }
+	void *get_void_ptr() { return (void *)(typename std::remove_reference<decltype(((T*)nullptr)->data())>::type*)_uorb_sub; }
+
+	int getUORBHandle() { return _uorb_sub->getHandle(); }
+
+protected:
+	uORB::SubscriptionBase * _uorb_sub;	/**< Handle to the subscription */
+	typename std::remove_reference<decltype(((T*)nullptr)->data())>::type getUORBData() { return (typename std::remove_reference<decltype(((T*)nullptr)->data())>::type)*_uorb_sub; }
 };
 
-template<typename M>
+//XXX reduce to one class with overloaded constructor?
+template<typename T>
 class SubscriberUORBCallback :
-	public SubscriberUORB<M>
+	public SubscriberUORB<T>
 {
 public:
 	/**
@@ -228,11 +267,16 @@ public:
 	 * @param interval	Minimal interval between calls to callback
 	 * @param list	    subscriber is added to this list
 	 */
-	SubscriberUORBCallback(const struct orb_metadata *meta,
-			       unsigned interval,
-			       std::function<void(const M &)> callback,
-			       List<uORB::SubscriptionNode *> *list) :
-		SubscriberUORB<M>(meta, interval, list),
+	// SubscriberUORBCallback(const struct orb_metadata *meta,
+			       // unsigned interval,
+			       // std::function<void(const M &)> callback,
+			       // List<uORB::SubscriptionNode *> *list) :
+		// SubscriberUORB<M>(meta, interval, list),
+		// _callback(callback)
+	// {}
+	SubscriberUORBCallback(uORB::SubscriptionBase * uorb_sub,
+			       std::function<void(typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &)> callback) :
+		SubscriberUORB<T>(uorb_sub),
 		_callback(callback)
 	{}
 
@@ -245,13 +289,13 @@ public:
 	 */
 	virtual void update()
 	{
-		if (!uORB::Subscription<M>::updated()) {
+		if (!SubscriberUORB<T>::_uorb_sub->updated()) {
 			/* Topic not updated, do not call callback */
 			return;
 		}
 
 		/* get latest data */
-		uORB::Subscription<M>::update();
+		SubscriberUORB<T>::_uorb_sub->update();
 
 
 		/* Check if there is a callback */
@@ -260,12 +304,12 @@ public:
 		}
 
 		/* Call callback which performs actions based on this data */
-		_callback(uORB::Subscription<M>::getData());
+		_callback(SubscriberUORB<T>::getUORBData());
 
 	};
 
 protected:
-	std::function<void(const M &)> _callback;	/**< Callback handle,
+	std::function<void(const typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &)> _callback;	/**< Callback handle,
 							  called when new data is available */
 };
 #endif
diff --git a/src/platforms/ros/px4_messages/px4_rc_channels.h b/src/platforms/ros/px4_messages/px4_rc_channels.h
new file mode 100644
index 000000000..a5a5ee202
--- /dev/null
+++ b/src/platforms/ros/px4_messages/px4_rc_channels.h
@@ -0,0 +1,25 @@
+#include "px4/rc_channels.h"
+#include "platforms/px4_message.h"
+
+#pragma once
+namespace px4
+{
+
+class  px4_rc_channels :
+	public PX4Message<rc_channels>
+{
+public:
+	px4_rc_channels() :
+		PX4Message<rc_channels>()
+	{}
+
+	px4_rc_channels(rc_channels msg) :
+		PX4Message<rc_channels>(msg)
+	{}
+
+	~px4_rc_channels() {}
+
+	PX4TopicHandle handle() {return "rc_channels";}
+};
+
+}