added a messageplayer prototype for ros

CMakeLists.txt

@@ -116,6 +116,7 @@ catkin_package(
 include_directories(
   ${catkin_INCLUDE_DIRS}
   src/platforms
+  src/platforms/ros/px4_messages
   src/include
   src/modules
   src/
@@ -157,52 +158,52 @@ target_link_libraries(subscriber
   px4
 )
 
-## MC Attitude Control
+# ## MC Attitude Control
-add_executable(mc_att_control
+# add_executable(mc_att_control
-	src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp
+	# src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp
-	src/modules/mc_att_control_multiplatform/mc_att_control.cpp
+	# src/modules/mc_att_control_multiplatform/mc_att_control.cpp
-	src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp)
+	# src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp)
-add_dependencies(mc_att_control ${PROJECT_NAME}_generate_messages_cpp_cpp)
+# add_dependencies(mc_att_control ${PROJECT_NAME}_generate_messages_cpp_cpp)
-target_link_libraries(mc_att_control
+# target_link_libraries(mc_att_control
-  ${catkin_LIBRARIES}
+  # ${catkin_LIBRARIES}
-  px4
+  # px4
-)
+# )
 
-## Attitude Estimator dummy
+# ## Attitude Estimator dummy
-add_executable(attitude_estimator
+# add_executable(attitude_estimator
-	src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp)
+	# src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp)
-add_dependencies(attitude_estimator ${PROJECT_NAME}_generate_messages_cpp_cpp)
+# add_dependencies(attitude_estimator ${PROJECT_NAME}_generate_messages_cpp_cpp)
-target_link_libraries(attitude_estimator
+# target_link_libraries(attitude_estimator
-  ${catkin_LIBRARIES}
+  # ${catkin_LIBRARIES}
-  px4
+  # px4
-)
+# )
 
-## Manual input
+# ## Manual input
-add_executable(manual_input
+# add_executable(manual_input
-	src/platforms/ros/nodes/manual_input/manual_input.cpp)
+	# src/platforms/ros/nodes/manual_input/manual_input.cpp)
-add_dependencies(manual_input ${PROJECT_NAME}_generate_messages_cpp_cpp)
+# add_dependencies(manual_input ${PROJECT_NAME}_generate_messages_cpp_cpp)
-target_link_libraries(manual_input
+# target_link_libraries(manual_input
-  ${catkin_LIBRARIES}
+  # ${catkin_LIBRARIES}
-  px4
+  # px4
-)
+# )
 
-## Multicopter Mixer dummy
+# ## Multicopter Mixer dummy
-add_executable(mc_mixer
+# add_executable(mc_mixer
-  src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp)
+  # src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp)
-add_dependencies(mc_mixer ${PROJECT_NAME}_generate_messages_cpp_cpp)
+# add_dependencies(mc_mixer ${PROJECT_NAME}_generate_messages_cpp_cpp)
-target_link_libraries(mc_mixer
+# target_link_libraries(mc_mixer
-  ${catkin_LIBRARIES}
+  # ${catkin_LIBRARIES}
-  px4
+  # px4
-)
+# )
 
-## Commander
+# ## Commander
-add_executable(commander
+# add_executable(commander
-	src/platforms/ros/nodes/commander/commander.cpp)
+	# src/platforms/ros/nodes/commander/commander.cpp)
-add_dependencies(manual_input ${PROJECT_NAME}_generate_messages_cpp_cpp)
+# add_dependencies(manual_input ${PROJECT_NAME}_generate_messages_cpp_cpp)
-target_link_libraries(commander
+# target_link_libraries(commander
-  ${catkin_LIBRARIES}
+  # ${catkin_LIBRARIES}
-  px4
+  # px4
-)
+# )
 
 #############
 ## Install ##

makefiles/config_px4fmu-v2_multiplatform.mk

@@ -86,7 +86,8 @@ MODULES		+= modules/position_estimator_inav
 #MODULES		+= modules/fw_pos_control_l1
 #MODULES		+= modules/fw_att_control
 # MODULES		+= modules/mc_att_control
-MODULES		+= modules/mc_att_control_multiplatform
+# MODULES		+= modules/mc_att_control_multiplatform
+MODULES		+= examples/publisher
 MODULES		+= modules/mc_pos_control
 MODULES 	+= modules/vtol_att_control
 

makefiles/config_px4fmu-v2_test.mk

@@ -56,7 +56,7 @@ MODULES		+= systemcmds/ver
 # Example modules
 #
 MODULES		+= examples/matlab_csv_serial
-MODULES		+= examples/subscriber
+#MODULES		+= examples/subscriber
 MODULES		+= examples/publisher
 
 #

msg/templates/msg.h.template

@@ -95,17 +95,17 @@ for field in spec.parsed_fields():
 @##############################
 @{
 
-type_map = {'int8': 'int8_t',
+type_map = {'int8': ['int8_t', '0'],
-  'int16': 'int16_t',
+  'int16': ['int16_t', '0'],
-  'int32': 'int32_t',
+  'int32': ['int32_t', '0'],
-  'int64': 'int64_t',
+  'int64': ['int64_t', '0'],
-  'uint8': 'uint8_t',
+  'uint8': ['uint8_t', '0'],
-  'uint16': 'uint16_t',
+  'uint16': ['uint16_t', '0'],
-  'uint32': 'uint32_t',
+  'uint32': ['uint32_t', '0'],
-  'uint64': 'uint64_t',
+  'uint64': ['uint64_t', '0'],
-  'float32': 'float',
+  'float32': ['float', '0.0f'],
-  'bool': 'bool',
+  'bool': ['bool', 'false'],
-  'fence_vertex': 'fence_vertex'}
+  'fence_vertex': ['fence_vertex', '']}
 
 # Function to print a standard ros type
 def print_field_def(field):
@@ -129,20 +129,70 @@ def print_field_def(field):
 
   if type in type_map:
     # need to add _t: int8 --> int8_t
-    type_px4 = type_map[type]
+    type_px4 = type_map[type][0]
   else:
     raise Exception("Type {0} not supported, add to to template file!".format(type))
 
   print('\t%s%s%s %s%s;'%(type_prefix, type_px4, type_appendix, field.name, array_size))
 
+# Function to init fields
+def get_field_init(field):
+  type = field.type
+  # detect embedded types
+  sl_pos = type.find('/')
+  type_appendix = ''
+  type_prefix = ''
+  if (sl_pos >= 0):
+    type = type[sl_pos + 1:]
+    type_prefix = 'struct '
+    type_appendix = '_s'
+
+  # detect arrays
+  a_pos = type.find('[')
+  array_size = ''
+  if (a_pos >= 0):
+    # field is array
+    array_size = type[a_pos:]
+    type = type[:a_pos]
+    return '\n\t%s{},'%(field.name)
+
+  if type in type_map:
+    # need to add _t: int8 --> int8_t
+    type_px4 = type_map[type][0]
+    init_value = type_map[type][1]
+  else:
+    raise Exception("Type {0} not supported, add to to template file!".format(type))
+
+  return '\n\t%s(%s),'%(field.name, init_value)
 }
+@#ifdef __cplusplus
+@#class @(spec.short_name)_s {
+@#public:
+@#else
 struct @(spec.short_name)_s {
+@#endif
 @{
 # loop over all fields and print the type and name
 for field in spec.parsed_fields():
   if (not field.is_header):
     print_field_def(field)
 }@
+
+@##ifdef __cplusplus
+@#@(spec.short_name)_s() :
+@#@{
+@#field_init = ''
+@## loop over all fields and init
+@#for field in spec.parsed_fields():
+@#  if (not field.is_header):
+@#    field_init += get_field_init(field)
+@#
+@#print(field_init[:-1])
+@#}@
+@#{}
+@#virtual ~@(spec.short_name)_s() {}
+@##endif
+
 };
 
 /**

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;
+		// PX4_TOPIC_T(rc_channels) msg;
-		msg.timestamp_last_valid = px4::get_time_micros();
+		// msg.timestamp_last_valid = px4::get_time_micros();
-		PX4_INFO("%llu", msg.timestamp_last_valid);
+		// 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();

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;
 };

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);
+	// PX4_SUBSCRIBE(_n, rc_channels, rc_channels_callback_function, _interval);
-	/* Class Method */
+	_n.subscribe<px4_rc_channels>(rc_channels_callback_function);
-	PX4_SUBSCRIBE(_n, rc_channels, SubscriberExample::rc_channels_callback, this, 1000);
+
-	/* No callback */
+	// [> Class Method <]
-	_sub_rc_chan = PX4_SUBSCRIBE(_n, rc_channels, 500);
+	// 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);
 }

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);
 

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);}
+};
+
+}

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>

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;
+};
+
+}

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>
+	// template<typename M>
-	Subscriber<M> *subscribe(const char *topic, void(*fp)(const 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));
+		SubscriberBase *sub = new SubscriberROS<T>(std::bind(fp, std::placeholders::_1));
-		ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS<M>::callback,
+		ros::Subscriber ros_sub = ros::NodeHandle::subscribe((new T())->handle(), kQueueSizeDefault,
-					  (SubscriberROS<M> *)sub);
+				&SubscriberROS<T>::callback, (SubscriberROS<T> *)sub);
-		((SubscriberROS<M> *)sub)->set_ros_sub(ros_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>
+	// template<typename M, typename T>
-	Subscriber<M> *subscribe(const char *topic, void(T::*fp)(const M &), T *obj)
+	// 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));
+		// SubscriberBase *sub = new SubscriberROS<M>(std::bind(fp, obj, std::placeholders::_1));
-		ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS<M>::callback,
+		// ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS<M>::callback,
-					  (SubscriberROS<M> *)sub);
+					  // (SubscriberROS<M> *)sub);
-		((SubscriberROS<M> *)sub)->set_ros_sub(ros_sub);
+		// ((SubscriberROS<M> *)sub)->set_ros_sub(ros_sub);
-		_subs.push_back(sub);
+		// _subs.push_back(sub);
-		return (Subscriber<M> *)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>
+	// template<typename M>
-	Subscriber<M> *subscribe(const char *topic)
+	// Subscriber<M> *subscribe(const char *topic)
-	{
+	// {
-		SubscriberBase *sub = new SubscriberROS<M>();
+		// SubscriberBase *sub = new SubscriberROS<M>();
-		ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS<M>::callback,
+		// ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS<M>::callback,
-					  (SubscriberROS<M> *)sub);
+					  // (SubscriberROS<M> *)sub);
-		((SubscriberROS<M> *)sub)->set_ros_sub(ros_sub);
+		// ((SubscriberROS<M> *)sub)->set_ros_sub(ros_sub);
-		_subs.push_back(sub);
+		// _subs.push_back(sub);
-		return (Subscriber<M> *)sub;
+		// return (Subscriber<M> *)sub;
-	}
+	// }
 
 	/**
 	 * Advertise topic
 	 * @param topic		Name of the topic
 	 */
-	template<typename M>
+	// template<typename M>
-	Publisher *advertise(const char *topic)
+	// 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>
+	// template<typename M>
-	Subscriber<M> *subscribe(const struct orb_metadata *meta,
+	// Subscriber<M> *subscribe(const struct orb_metadata *meta,
-				 std::function<void(const M &)> callback,
+				 // std::function<void(const M &)> callback,
-				 unsigned interval)
+				 // 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>
+	// template<typename M>
-	Subscriber<M> *subscribe(const struct orb_metadata *meta,
+	// Subscriber<M> *subscribe(const struct orb_metadata *meta,
-				 unsigned interval)
+				 // unsigned interval)
-	{
+	// {
-		SubscriberUORB<M> *sub_px4 = new SubscriberUORB<M>(meta, interval, &_subs);
+		// SubscriberUORB<M> *sub_px4 = new SubscriberUORB<M>(meta, interval, &_subs);
 
-		/* Check if this is the smallest interval so far and update _sub_min_interval */
+		// [> 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->getInterval() > sub_px4->getInterval()) {
-			_sub_min_interval = sub_px4;
+			// _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>
+	// template<typename M>
-	Publisher *advertise(const struct orb_metadata *meta)
+	// 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<SubscriberNode *> _subs;		/**< Subcriptions of node */
-	List<uORB::PublicationNode *> _pubs;		/**< Publications of node */
+	List<Publisher *> _pubs;		/**< Publications of node */
-	uORB::SubscriptionNode *_sub_min_interval;	/**< Points to the sub wtih the smallest interval
+	SubscriberNode *_sub_min_interval;	/**< Points to the sub wtih the smallest interval
 							  of all Subscriptions in _subs*/
 };
 #endif

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 uORB::PublicationNode,
-	public PublisherBase
+	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,
+	// Publisher(const struct orb_metadata *meta,
-		  List<uORB::PublicationNode *> *list) :
+		  // List<uORB::PublicationNode *> *list) :
-		uORB::PublicationNode(meta, list),
+		// uORB::PublicationNode(meta, list),
-		PublisherBase()
+		// 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
 }

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) :
+	SubscriberROS(std::function<void(const typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &)> cbf) :
-		Subscriber<M>(),
+		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 */
+	std::function<void(const typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &)> _cbf;	/**< Callback that the user provided on the subscription */
-	M _msg_current;				/**< Current Message value */
+	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 Subscriber<T>,
-	public uORB::Subscription<M>
+	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,
+	// SubscriberUORB(const struct orb_metadata *meta,
-		       unsigned interval,
+		       // unsigned interval,
-		       List<uORB::SubscriptionNode *> *list) :
+		       // List<uORB::SubscriptionNode *> *list) :
-		Subscriber<M>(),
+		// Subscriber<M>(),
-		uORB::Subscription<M>(meta, interval, list)
+		// 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,
+	// SubscriberUORBCallback(const struct orb_metadata *meta,
-			       unsigned interval,
+			       // unsigned interval,
-			       std::function<void(const M &)> callback,
+			       // std::function<void(const M &)> callback,
-			       List<uORB::SubscriptionNode *> *list) :
+			       // List<uORB::SubscriptionNode *> *list) :
-		SubscriberUORB<M>(meta, interval, 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

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";}
+};
+
+}