From caa61a4fdc7898987da7a03e1924ced8962bb92c Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Fri, 28 Nov 2014 23:09:45 +0100
Subject: add support for subcription method callbacks for ros and nuttx

---
 src/platforms/px4_defines.h    | 15 ++++++++-------
 src/platforms/px4_nodehandle.h |  9 +++++++++
 2 files changed, 17 insertions(+), 7 deletions(-)

(limited to 'src/platforms')

diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h
index 7efe13bae..f6679d01b 100644
--- a/src/platforms/px4_defines.h
+++ b/src/platforms/px4_defines.h
@@ -44,27 +44,28 @@
  * Building for running within the ROS environment
  */
 #define __EXPORT
-// #define PX4_MAIN_FUNCTION(_prefix)
 #define PX4_MAIN_FUNCTION(_prefix) int main(int argc, char **argv)
 #define PX4_WARN ROS_WARN
 #define PX4_INFO ROS_INFO
 #define PX4_TOPIC(_name) #_name
 #define PX4_TOPIC_T(_name) _name
-#define PX4_SUBSCRIBE(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), _cbf);
+#define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _obj, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), &_cbf, &_obj);
+#define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), _cbf);
 
 #else
 /*
  * Building for NuttX
  */
-
-// #define PX4_MAIN_FUNCTION(_prefix) __EXPORT int _prefix##_main(int argc, char **argv)() { return main(argc, argv); }
-// #define PX4_MAIN_FUNCTION(_prefix) extern "C" __EXPORT int _prefix##_main(int argc, char *argv[]) { return main(argc, argv); }
 #define PX4_MAIN_FUNCTION(_prefix) extern "C" __EXPORT int _prefix##_main(int argc, char *argv[])
 #define PX4_WARN warnx
 #define PX4_WARN warnx
 #define PX4_INFO warnx
 #define PX4_TOPIC(_name) ORB_ID(_name)
 #define PX4_TOPIC_T(_name) _name##_s
-#define PX4_SUBSCRIBE(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe<PX4_TOPIC_T(_name)>(PX4_TOPIC(_name), [](const PX4_TOPIC_T(_name)& msg){ return _cbf(msg);}, _interval)
-
+#define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _obj, _interval) _nodehandle.subscribe<PX4_TOPIC_T(_name)>(PX4_TOPIC(_name), std::bind(&_cbf, _obj, std::placeholders::_1), _interval)
+#define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe<PX4_TOPIC_T(_name)>(PX4_TOPIC(_name), std::bind(&_cbf, std::placeholders::_1), _interval)
 #endif
+
+/* Overload the PX4_SUBSCRIBE macro to suppport methods and pure functions as callback */
+#define PX4_GET_SUBSCRIBE(_1, _2, _3, _4, _5, NAME, ...) NAME
+#define PX4_SUBSCRIBE(...) PX4_GET_SUBSCRIBE(__VA_ARGS__, PX4_SUBSCRIBE_CBMETH, PX4_SUBSCRIBE_CBFUNC)(__VA_ARGS__)
diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h
index 45ef225fa..415351756 100644
--- a/src/platforms/px4_nodehandle.h
+++ b/src/platforms/px4_nodehandle.h
@@ -71,6 +71,7 @@ public:
 		//XXX empty lists
 	};
 
+	/* Constructor with callback to function */
 	template<typename M>
 	Subscriber * subscribe(const char *topic, void(*fp)(M)) {
 		ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, fp);
@@ -78,6 +79,14 @@ public:
 		_subs.push_back(sub);
 		return sub;
 	}
+	/* Constructor with callback to class method */
+	template<typename M, typename T>
+	Subscriber * subscribe(const char *topic, void(T::*fp)(M), T *obj) {
+		ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, fp, obj);
+		Subscriber * sub = new Subscriber(ros_sub);
+		_subs.push_back(sub);
+		return sub;
+	}
 
 	template<typename M>
 	Publisher * advertise(const char *topic) {
-- 
cgit v1.2.3