fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

add support for subcription method callbacks for ros and nuttx

Browse Source
This commit is contained in:
Thomas Gubler 2014-11-28 23:09:45 +01:00
parent 9abc8e26b7
commit caa61a4fdc
3 changed files with 40 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
src/examples/subscriber/subscriber.cpp
View File

@ -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");
/**

15
src/platforms/px4_defines.h
View File

@ -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__)

9
src/platforms/px4_nodehandle.h
View File

@ -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) {