|
|
|
@ -66,7 +66,8 @@ public:
@@ -66,7 +66,8 @@ public:
|
|
|
|
|
_pubs() |
|
|
|
|
{} |
|
|
|
|
|
|
|
|
|
~NodeHandle() { |
|
|
|
|
~NodeHandle() |
|
|
|
|
{ |
|
|
|
|
//XXX empty lists
|
|
|
|
|
}; |
|
|
|
|
|
|
|
|
@ -76,9 +77,10 @@ public:
@@ -76,9 +77,10 @@ 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); |
|
|
|
|
Subscriber *sub = new Subscriber(ros_sub); |
|
|
|
|
_subs.push_back(sub); |
|
|
|
|
return sub; |
|
|
|
|
} |
|
|
|
@ -89,9 +91,10 @@ public:
@@ -89,9 +91,10 @@ 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); |
|
|
|
|
Subscriber *sub = new Subscriber(ros_sub); |
|
|
|
|
_subs.push_back(sub); |
|
|
|
|
return 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); |
|
|
|
@ -121,8 +125,8 @@ public:
@@ -121,8 +125,8 @@ public:
|
|
|
|
|
|
|
|
|
|
private: |
|
|
|
|
static const uint32_t kQueueSizeDefault = 1000; /**< Size of queue for ROS */ |
|
|
|
|
std::list<Subscriber*> _subs; /**< Subcriptions of node */ |
|
|
|
|
std::list<Publisher*> _pubs; /**< Publications of node */ |
|
|
|
|
std::list<Subscriber *> _subs; /**< Subcriptions of node */ |
|
|
|
|
std::list<Publisher *> _pubs; /**< Publications of node */ |
|
|
|
|
}; |
|
|
|
|
#else |
|
|
|
|
class __EXPORT NodeHandle |
|
|
|
@ -144,16 +148,18 @@ public:
@@ -144,16 +148,18 @@ public:
|
|
|
|
|
*/ |
|
|
|
|
|
|
|
|
|
template<typename M> |
|
|
|
|
Subscriber * subscribe(const struct orb_metadata *meta, |
|
|
|
|
std::function<void(const M&)> callback, |
|
|
|
|
unsigned interval) { |
|
|
|
|
Subscriber *subscribe(const struct orb_metadata *meta, |
|
|
|
|
std::function<void(const M &)> callback, |
|
|
|
|
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; |
|
|
|
|
|
|
|
|
|
return (Subscriber *)sub_px4; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
/**
|
|
|
|
@ -161,16 +167,18 @@ public:
@@ -161,16 +167,18 @@ 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); |
|
|
|
|
Publisher *pub = new Publisher(meta, &_pubs); |
|
|
|
|
return pub; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
/**
|
|
|
|
|
* 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; |
|
|
|
@ -212,9 +223,9 @@ public:
@@ -212,9 +223,9 @@ public:
|
|
|
|
|
} |
|
|
|
|
private: |
|
|
|
|
static const uint16_t kMaxSubscriptions = 100; |
|
|
|
|
List<uORB::SubscriptionNode*> _subs; /**< Subcriptions of node */ |
|
|
|
|
List<uORB::PublicationNode*> _pubs; /**< Publications of node */ |
|
|
|
|
uORB::SubscriptionNode* _sub_min_interval; /**< Points to the sub wtih the smallest interval
|
|
|
|
|
List<uORB::SubscriptionNode *> _subs; /**< Subcriptions of node */ |
|
|
|
|
List<uORB::PublicationNode *> _pubs; /**< Publications of node */ |
|
|
|
|
uORB::SubscriptionNode *_sub_min_interval; /**< Points to the sub wtih the smallest interval
|
|
|
|
|
of all Subscriptions in _subs*/ |
|
|
|
|
}; |
|
|
|
|
#endif |
|
|
|
|