diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2014-12-04 13:20:12 +0100 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-12-04 13:20:12 +0100 |
commit | f4a326c2bf7af6eac86983cd65e66ff76e623e22 (patch) | |
tree | 53e5c4937de05fcf46aa14882ecf1f2bf8660250 /src/platforms/px4_nodehandle.h | |
parent | e0bb713bb03c0aa79e69496c787c012a8e1b78d1 (diff) | |
download | px4-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.h | 49 |
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 |