diff options
| -rw-r--r-- | src/examples/subscriber/subscriber.cpp | 30 | ||||
| -rw-r--r-- | src/platforms/px4_defines.h | 15 | ||||
| -rw-r--r-- | src/platforms/px4_nodehandle.h | 9 |
3 files changed, 40 insertions, 14 deletions
diff --git a/src/examples/subscriber/subscriber.cpp b/src/examples/subscriber/subscriber.cpp index a186ba94a..3d54a1a2f 100644 --- a/src/examples/subscriber/subscriber.cpp +++ b/src/examples/subscriber/subscriber.cpp @@ -32,21 +32,29 @@ using namespace px4; /** * This tutorial demonstrates simple receipt of messages over the PX4 middleware system. */ -void rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg) -{ +void rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg) { PX4_INFO("I heard: [%llu]", msg.timestamp_last_valid); } -void rc_channels_callback2(const PX4_TOPIC_T(rc_channels) &msg) -{ - PX4_INFO("I heard2: [%llu]", msg.timestamp_last_valid); +void rc_channels_callback2(const PX4_TOPIC_T(rc_channels) &msg) { + PX4_INFO("I heard(2): [%llu]", msg.timestamp_last_valid); } + +class RCHandler { +public: + void callback(const PX4_TOPIC_T(rc_channels) &msg) { + PX4_INFO("RCHandler class heard: [%llu]", msg.timestamp_last_valid); + } +}; + +RCHandler rchandler; + + namespace px4 { bool task_should_exit = false; } -PX4_MAIN_FUNCTION(subscriber) -{ +PX4_MAIN_FUNCTION(subscriber) { /** * The ros::init() function needs to see argc and argv so that it can perform * any ROS arguments and name remapping that were provided at the command line. For programmatic @@ -83,6 +91,14 @@ PX4_MAIN_FUNCTION(subscriber) */ PX4_SUBSCRIBE(n, rc_channels, rc_channels_callback, 100); PX4_SUBSCRIBE(n, rc_channels, rc_channels_callback2, 1000); + //1 + // PX4_SUBSCRIBE(n, rc_channels, callee.rc_channels_callback, , 1000); + //2 + // PX4_SUBSCRIBE(n, rc_channels, rchandler.callback, &rchandler, 1000); + //3 for bind + PX4_SUBSCRIBE(n, rc_channels, RCHandler::callback, rchandler, 1000); + // ros::NodeHandle n2; + // n2.subscribe("chatter", 1000, &RCHandler::callback, &rchandler); PX4_INFO("subscribed"); /** 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) { |