aboutsummaryrefslogtreecommitdiff
path: root/src/platforms/px4_nodehandle.h
diff options
context:
space:
mode:
authorThomas Gubler <thomasgubler@gmail.com>2014-12-04 13:20:12 +0100
committerThomas Gubler <thomasgubler@gmail.com>2014-12-04 13:20:12 +0100
commitf4a326c2bf7af6eac86983cd65e66ff76e623e22 (patch)
tree53e5c4937de05fcf46aa14882ecf1f2bf8660250 /src/platforms/px4_nodehandle.h
parente0bb713bb03c0aa79e69496c787c012a8e1b78d1 (diff)
downloadpx4-firmware-f4a326c2bf7af6eac86983cd65e66ff76e623e22.tar.gz
px4-firmware-f4a326c2bf7af6eac86983cd65e66ff76e623e22.tar.bz2
px4-firmware-f4a326c2bf7af6eac86983cd65e66ff76e623e22.zip
platform headers: fix code style
Diffstat (limited to 'src/platforms/px4_nodehandle.h')
-rw-r--r--src/platforms/px4_nodehandle.h49
1 files changed, 30 insertions, 19 deletions
diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h
index 1762e1656..5b7247b20 100644
--- a/src/platforms/px4_nodehandle.h
+++ b/src/platforms/px4_nodehandle.h
@@ -66,7 +66,8 @@ public:
_pubs()
{}
- ~NodeHandle() {
+ ~NodeHandle()
+ {
//XXX empty lists
};
@@ -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:
* @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:
* @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:
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:
*/
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:
* @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:
/**
* 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:
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:
}
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