|
|
|
@ -66,7 +66,8 @@ public:
@@ -66,7 +66,8 @@ public:
|
|
|
|
|
_pubs() |
|
|
|
|
{} |
|
|
|
|
|
|
|
|
|
~NodeHandle() { |
|
|
|
|
~NodeHandle() |
|
|
|
|
{ |
|
|
|
|
//XXX empty lists
|
|
|
|
|
}; |
|
|
|
|
|
|
|
|
@ -76,7 +77,8 @@ public:
@@ -76,7 +77,8 @@ 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)(M)) |
|
|
|
|
{ |
|
|
|
|
ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, fp); |
|
|
|
|
Subscriber *sub = new Subscriber(ros_sub); |
|
|
|
|
_subs.push_back(sub); |
|
|
|
@ -89,7 +91,8 @@ public:
@@ -89,7 +91,8 @@ 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)(M), T *obj) |
|
|
|
|
{ |
|
|
|
|
ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, fp, obj); |
|
|
|
|
Subscriber *sub = new Subscriber(ros_sub); |
|
|
|
|
_subs.push_back(sub); |
|
|
|
@ -101,7 +104,8 @@ public:
@@ -101,7 +104,8 @@ public:
|
|
|
|
|
* @param topic Name of the topic |
|
|
|
|
*/ |
|
|
|
|
template<typename M> |
|
|
|
|
Publisher * advertise(const char *topic) { |
|
|
|
|
Publisher *advertise(const char *topic) |
|
|
|
|
{ |
|
|
|
|
ros::Publisher ros_pub = ros::NodeHandle::advertise<M>(topic, kQueueSizeDefault); |
|
|
|
|
Publisher *pub = new Publisher(ros_pub); |
|
|
|
|
_pubs.push_back(pub); |
|
|
|
@ -146,13 +150,15 @@ public:
@@ -146,13 +150,15 @@ public:
|
|
|
|
|
template<typename M> |
|
|
|
|
Subscriber *subscribe(const struct orb_metadata *meta, |
|
|
|
|
std::function<void(const M &)> callback, |
|
|
|
|
unsigned interval) { |
|
|
|
|
unsigned interval) |
|
|
|
|
{ |
|
|
|
|
SubscriberPX4<M> *sub_px4 = new SubscriberPX4<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()) { |
|
|
|
|
_sub_min_interval = sub_px4; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
return (Subscriber *)sub_px4; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
@ -161,7 +167,8 @@ public:
@@ -161,7 +167,8 @@ public:
|
|
|
|
|
* @param meta Describes the topic which is advertised |
|
|
|
|
*/ |
|
|
|
|
template<typename M> |
|
|
|
|
Publisher * advertise(const struct orb_metadata *meta) { |
|
|
|
|
Publisher *advertise(const struct orb_metadata *meta) |
|
|
|
|
{ |
|
|
|
|
//XXX
|
|
|
|
|
Publisher *pub = new Publisher(meta, &_pubs); |
|
|
|
|
return pub; |
|
|
|
@ -170,7 +177,8 @@ public:
@@ -170,7 +177,8 @@ public:
|
|
|
|
|
/**
|
|
|
|
|
* Calls all callback waiting to be called |
|
|
|
|
*/ |
|
|
|
|
void spinOnce() { |
|
|
|
|
void spinOnce() |
|
|
|
|
{ |
|
|
|
|
/* Loop through subscriptions, call callback for updated subscriptions */ |
|
|
|
|
uORB::SubscriptionNode *sub = _subs.getHead(); |
|
|
|
|
int count = 0; |
|
|
|
@ -189,9 +197,11 @@ public:
@@ -189,9 +197,11 @@ public:
|
|
|
|
|
/**
|
|
|
|
|
* Keeps calling callbacks for incomming messages, returns when module is terminated |
|
|
|
|
*/ |
|
|
|
|
void spin() { |
|
|
|
|
void spin() |
|
|
|
|
{ |
|
|
|
|
while (ok()) { |
|
|
|
|
const int timeout_ms = 100; |
|
|
|
|
|
|
|
|
|
/* Only continue in the loop if the nodehandle has subscriptions */ |
|
|
|
|
if (_sub_min_interval == nullptr) { |
|
|
|
|
usleep(timeout_ms * 1000); |
|
|
|
@ -202,6 +212,7 @@ public:
@@ -202,6 +212,7 @@ public:
|
|
|
|
|
struct pollfd pfd; |
|
|
|
|
pfd.fd = _sub_min_interval->getHandle(); |
|
|
|
|
pfd.events = POLLIN; |
|
|
|
|
|
|
|
|
|
if (poll(&pfd, 1, timeout_ms) <= 0) { |
|
|
|
|
/* timed out */ |
|
|
|
|
continue; |
|
|
|
|