From 978013bbb8d67e295d92a54e16f7728013722e92 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 25 Nov 2014 09:56:18 +0100
Subject: [PATCH] px4 wrapper for ros publisher

---
 src/examples/publisher/publisher.cpp |  5 +++--
 src/platforms/px4_nodehandle.h       | 16 +++++++++++++---
 src/platforms/px4_publisher.h        | 18 +++++++++++++++++-
 3 files changed, 33 insertions(+), 6 deletions(-)

diff --git a/src/examples/publisher/publisher.cpp b/src/examples/publisher/publisher.cpp
index 91e0631622..6869e765bd 100644
--- a/src/examples/publisher/publisher.cpp
+++ b/src/examples/publisher/publisher.cpp
@@ -38,7 +38,7 @@ int main(int argc, char **argv)
 
 	px4::init(argc, argv, "px4_publisher");
 
-	ros::NodeHandle n;
+	px4::NodeHandle n;
 
 	/**
 	 * The advertise() function is how you tell ROS that you want to
@@ -57,7 +57,8 @@ 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.
 	 */
-	ros::Publisher rc_channels_pub = n.advertise<px4::rc_channels>("rc_channels", 1000);
+	px4::Publisher rc_channels_pub = n.advertise<px4::rc_channels>("rc_channels");
+
 
 	px4::Rate loop_rate(10);
 
diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h
index 6880d74f65..bfda636b0f 100644
--- a/src/platforms/px4_nodehandle.h
+++ b/src/platforms/px4_nodehandle.h
@@ -38,6 +38,7 @@
  */
 #pragma once
 #include <px4_subscriber.h>
+#include <px4_publisher.h>
 #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__))
 #include "ros/ros.h"
 #include <list>
@@ -52,19 +53,28 @@ class NodeHandle : private ros::NodeHandle
 public:
 	NodeHandle () :
 		ros::NodeHandle(),
-		_subs()
+		_subs(),
+		_pubs()
 	{}
 
-	template<class M>
+	template<typename M>
 	Subscriber subscribe(const char *topic, void(*fp)(M)) {
 		ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, QUEUE_SIZE_DEFAULT, fp);
-		//XXX create list here, for ros and nuttx
 		Subscriber sub(ros_sub);
 		_subs.push_back(sub);
 		return sub;
 	}
+
+	template<typename M>
+	Publisher advertise(const char *topic) {
+		ros::Publisher ros_pub = ros::NodeHandle::advertise<M>(topic, QUEUE_SIZE_DEFAULT);
+		Publisher pub(ros_pub);
+		_pubs.push_back(pub);
+		return pub;
+	}
 private:
 	std::list<Subscriber> _subs;
+	std::list<Publisher> _pubs;
 };
 #else
 class NodeHandle
diff --git a/src/platforms/px4_publisher.h b/src/platforms/px4_publisher.h
index 78d2a744b7..7995251902 100644
--- a/src/platforms/px4_publisher.h
+++ b/src/platforms/px4_publisher.h
@@ -37,11 +37,27 @@
  * PX4 Middleware Wrapper Node Handle
  */
 #pragma once
+#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__))
+#include "ros/ros.h"
+#endif
 
 namespace px4
 {
+#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__))
 class Publisher
 {
-
+private:
+	ros::Publisher _ros_pub;
+public:
+	Publisher(ros::Publisher ros_pub) : _ros_pub(ros_pub)
+	{}
+	~Publisher() {};
+	template<typename M>
+	int publish(const M &msg) { _ros_pub.publish(msg); return 0; }
 };
+#else
+class Publisher
+{
+};
+#endif
 }