subscription class for ros now stores last message to avoid manual copy and support subscription with no callbacks

5 files changed, 113 insertions, 45 deletions

diff --git a/CMakeLists.txt b/CMakeLists.txt
index f206a5aa2..489467db7 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,5 +1,6 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(px4)
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
 
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
diff --git a/src/examples/subscriber/subscriber_example.cpp b/src/examples/subscriber/subscriber_example.cpp
index 39d090752..95e208ca6 100644
--- a/src/examples/subscriber/subscriber_example.cpp
+++ b/src/examples/subscriber/subscriber_example.cpp
@@ -76,8 +76,7 @@ SubscriberExample::SubscriberExample() :
  * Also the current value of the _sub_rc_chan subscription is printed
  */
 void SubscriberExample::rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg) {
-	//XXX
-	// PX4_INFO("Subscriber callback: [%llu], value of _sub_rc_chan: [%llu]",
-			// msg.timestamp_last_valid,
-			// ((SubscriberPX4<PX4_TOPIC_T(rc_channels)> *)_sub_rc_chan)->timestamp_last_valid);
+	PX4_INFO("Subscriber callback: [%llu], value of _sub_rc_chan: [%llu]",
+			msg.timestamp_last_valid,
+			((PX4_SUBSCRIBER_T(rc_channels) *)_sub_rc_chan)->get_msg().timestamp_last_valid);
 }
diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h
index 819954d0e..fa3ef216a 100644
--- a/src/platforms/px4_defines.h
+++ b/src/platforms/px4_defines.h
@@ -70,7 +70,7 @@
 /* Subscribe and providing a function as callback (do not use directly, use PX4_SUBSCRIBE instead) */
 #define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), _cbf);
 /* Subscribe without a callback (do not use directly, use PX4_SUBSCRIBE instead) */
-#define PX4_SUBSCRIBE_NOCB(_nodehandle, _name, _interval) _nodehandle.subscribe<const PX4_TOPIC_T(_name)&>(PX4_TOPIC(_name));
+#define PX4_SUBSCRIBE_NOCB(_nodehandle, _name, _interval) _nodehandle.subscribe<PX4_TOPIC_T(_name)>(PX4_TOPIC(_name));
 
 /* Parameter handle datatype */
 typedef const char *px4_param_t;
@@ -93,6 +93,9 @@ static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, float value)
 /* Get value of parameter */
 #define PX4_PARAM_GET(_handle, _destpt) ros::param::get(_handle, *_destpt)
 
+/* Get a subscriber class type based on the topic name */
+#define PX4_SUBSCRIBER_T(_name) SubscriberROS<PX4_TOPIC_T(_name)>
+
 #else
 /*
  * Building for NuttX
@@ -130,6 +133,15 @@ typedef param_t px4_param_t;
 
 /* Get value of parameter */
 #define PX4_PARAM_GET(_handle, _destpt) param_get(_handle, _destpt)
+
+/* Get a subscriber class type based on the topic name */
+#define PX4_SUBSCRIBER_T(_name) SubscriberUORB<PX4_TOPIC_T(_name)>
+
+/* XXX this is a hack to resolve conflicts with NuttX headers */
+#define isspace(c) \
+  ((c) == ' '  || (c) == '\t' || (c) == '\n' || \
+   (c) == '\r' || (c) == '\f' || c== '\v')
+
 #endif
 
 /* Shortcut for subscribing to topics
@@ -146,7 +158,3 @@ typedef param_t px4_param_t;
 
 /* wrapper for rotation matrices stored in arrays */
 #define PX4_R(_array, _x, _y) PX4_ARRAY2D(_array, 3, _x, _y)
-
-#define isspace(c) \
-  ((c) == ' '  || (c) == '\t' || (c) == '\n' || \
-   (c) == '\r' || (c) == '\f' || c== '\v')
diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h
index 25b8e037d..c24a18303 100644
--- a/src/platforms/px4_nodehandle.h
+++ b/src/platforms/px4_nodehandle.h
@@ -77,10 +77,11 @@ public:
 	 * @param fb		Callback, executed on receiving a new message
 	 */
 	template<typename M>
-	Subscriber *subscribe(const char *topic, void(*fp)(M))
+	Subscriber *subscribe(const char *topic, void(*fp)(const M&))
 	{
-		ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, fp);
-		Subscriber *sub = new Subscriber(ros_sub);
+		Subscriber *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 sub;
 	}
@@ -91,10 +92,11 @@ public:
 	 * @param fb		Callback, executed on receiving a new message
 	 */
 	template<typename M, typename T>
-	Subscriber *subscribe(const char *topic, void(T::*fp)(M), T *obj)
+	Subscriber *subscribe(const char *topic, void(T::*fp)(const M&), T *obj)
 	{
-		ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, fp, obj);
-		Subscriber *sub = new Subscriber(ros_sub);
+		Subscriber *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 sub;
 	}
@@ -106,10 +108,11 @@ public:
 	template<typename M>
 	Subscriber *subscribe(const char *topic)
 	{
-		//XXX missing implementation
-		// Subscriber *sub = new Subscriber(ros_sub);
-		// _subs.push_back(sub);
-		return (Subscriber *)NULL;
+		Subscriber *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 sub;
 	}
 
 	/**
@@ -165,7 +168,7 @@ public:
 			      std::function<void(const M &)> callback,
 			      unsigned interval)
 	{
-		SubscriberPX4<M> *sub_px4 = new SubscriberPX4<M>(meta, interval, callback, &_subs);
+		SubscriberUORB<M> *sub_px4 = new SubscriberUORB<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()) {
diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h
index 45cb650b2..234b72f5b 100644
--- a/src/platforms/px4_subscriber.h
+++ b/src/platforms/px4_subscriber.h
@@ -36,6 +36,7 @@
  *
  * PX4 Middleware Wrapper Subscriber
  */
+#include <functional>
 #pragma once
 #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__))
 /* includes when building for ros */
@@ -44,59 +45,108 @@
 /* includes when building for NuttX */
 #include <uORB/Subscription.hpp>
 #include <containers/List.hpp>
-#include <functional>
+#include "px4_nodehandle.h"
 #endif
 
 namespace px4
 {
-
-#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__))
+/**
+ * Subscriber class which is used by nodehandle
+ */
 class Subscriber
 {
 public:
-	/**
-	 * Construct Subscriber by providing a ros::Subscriber
-	 * @param ros_sub the ros subscriber which will be used to perform the publications
-	 */
-	Subscriber(ros::Subscriber ros_sub) :
-		_ros_sub(ros_sub)
-	{}
-
+	Subscriber() {};
 	~Subscriber() {};
-private:
-	ros::Subscriber _ros_sub;	/**< Handle to ros subscriber */
 };
 
-#else
-
+#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__))
 /**
- * Subscriber class which is used by nodehandle when building for NuttX
+ * Subscriber class that is templated with the ros n message type
  */
-class Subscriber
+template<typename M>
+class SubscriberROS :
+	public Subscriber
 {
+friend class NodeHandle;
+
 public:
-	Subscriber() {};
-	~Subscriber() {};
-private:
+	/**
+	 * Construct Subscriber by providing a callback function
+	 */
+	SubscriberROS(std::function<void(const M &)> cbf) :
+		Subscriber(),
+		_ros_sub(),
+		_cbf(cbf),
+		_msg_current()
+	{}
+
+	/**
+	 * Construct Subscriber without a callback function
+	 */
+	SubscriberROS() :
+		Subscriber(),
+		_ros_sub(),
+		_cbf(NULL),
+		_msg_current()
+	{}
+
+
+	~SubscriberROS() {};
+
+	/* Accessors*/
+	/**
+	 * Get the last message value
+	 */
+	const M& get_msg() { return _msg_current; }
+
+protected:
+	/**
+	 * Called on topic update, saves the current message and then calls the provided callback function
+	 */
+	void callback(const M &msg) {
+		/* Store data */
+		_msg_current = msg;
+
+		/* Call callback */
+		if (_cbf != NULL) {
+			_cbf(msg);
+		}
+
+	}
+
+	/**
+	 * Saves the ros subscriber to keep ros subscription alive
+	 */
+	void set_ros_sub(ros::Subscriber ros_sub) {
+		_ros_sub = ros_sub;
+	}
+
+	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 */
+
 };
 
+#else
+
 /**
  * Subscriber class that is templated with the uorb subscription message type
  */
 template<typename M>
-class SubscriberPX4 :
+class SubscriberUORB :
 	public Subscriber,
 	public uORB::Subscription<M>
 {
 public:
 	/**
-	 * Construct SubscriberPX4 by providing orb meta data
+	 * Construct SubscriberUORB by providing orb meta data
 	 * @param meta	    orb metadata for the topic which is used
 	 * @param callback	Callback, executed on receiving a new message
 	 * @param interval	Minimal interval between calls to callback
 	 * @param list	    subscriber is added to this list
 	 */
-	SubscriberPX4(const struct orb_metadata *meta,
+	SubscriberUORB(const struct orb_metadata *meta,
 		      unsigned interval,
 		      std::function<void(const M &)> callback,
 		      List<uORB::SubscriptionNode *> *list) :
@@ -106,7 +156,7 @@ public:
 		//XXX store callback
 	{}
 
-	~SubscriberPX4() {};
+	~SubscriberUORB() {};
 
 	/**
 	 * Update Subscription
@@ -133,7 +183,14 @@ public:
 		_callback(uORB::Subscription<M>::getData());
 
 	};
-private:
+
+	/* Accessors*/
+	/**
+	 * Get the last message value
+	 */
+	const M& get_msg() { return uORB::Subscription<M>::getData(); }
+
+protected:
 	std::function<void(const M &)> _callback;	/**< Callback handle,
 							  called when new data is available */