WIP, towards more px4 compatibility, first macros

8 files changed, 69 insertions, 8 deletions

diff --git a/src/examples/publisher/publisher.cpp b/src/examples/publisher/publisher.cpp
index 6869e765b..c09cca1fc 100644
--- a/src/examples/publisher/publisher.cpp
+++ b/src/examples/publisher/publisher.cpp
@@ -57,7 +57,7 @@ int main(int argc, char **argv)
 	 * than we can send them, the number here specifies how many messages to
 	 * buffer up before throwing some away.
 	 */
-	px4::Publisher rc_channels_pub = n.advertise<px4::rc_channels>("rc_channels");
+	px4::Publisher rc_channels_pub = n.advertise<px4::rc_channels>(PX4_TOPIC(rc_channels));
 
 
 	px4::Rate loop_rate(10);
diff --git a/src/examples/subscriber/subscriber.cpp b/src/examples/subscriber/subscriber.cpp
index 828b1f937..b91859027 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(PX4_TOPIC(rc_channels), rc_channels_callback);
 	PX4_INFO("subscribed");
 
 	/**
diff --git a/src/include/px4.h b/src/include/px4.h
index bb97f2a8a..391972b12 100644
--- a/src/include/px4.h
+++ b/src/include/px4.h
@@ -46,14 +46,19 @@
  * Building for running within the ROS environment
  */
 #include "ros/ros.h"
+
 #define PX4_WARN ROS_WARN
 #define PX4_INFO ROS_INFO
+#define PX4_TOPIC(name) #name
 #else
 /*
  * Building for NuttX
  */
+#include <uORB/uORB.h>
+
 #define PX4_WARN warnx
 #define PX4_INFO warnx
+#define PX4_TOPIC(name) ORB_ID(name)
 #endif
 
 #include "../platforms/px4_defines.h"
diff --git a/src/modules/uORB/Publication.cpp b/src/modules/uORB/Publication.cpp
index cd0b30dd6..302b99c69 100644
--- a/src/modules/uORB/Publication.cpp
+++ b/src/modules/uORB/Publication.cpp
@@ -78,5 +78,6 @@ template class __EXPORT Publication<vehicle_rates_setpoint_s>;
 template class __EXPORT Publication<actuator_outputs_s>;
 template class __EXPORT Publication<encoders_s>;
 template class __EXPORT Publication<tecs_status_s>;
+template class __EXPORT Publication<rc_channels_s>;
 
 }
diff --git a/src/modules/uORB/Subscription.cpp b/src/modules/uORB/Subscription.cpp
index 44b6debc7..3406a66d3 100644
--- a/src/modules/uORB/Subscription.cpp
+++ b/src/modules/uORB/Subscription.cpp
@@ -101,5 +101,6 @@ template class __EXPORT Subscription<vehicle_local_position_setpoint_s>;
 template class __EXPORT Subscription<vehicle_local_position_s>;
 template class __EXPORT Subscription<vehicle_attitude_setpoint_s>;
 template class __EXPORT Subscription<vehicle_rates_setpoint_s>;
+template class __EXPORT Subscription<rc_channels_s>;
 
 } // namespace uORB
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