mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-23 16:23:34 +08:00
platform headers: fix code style
This commit is contained in:
@@ -71,14 +71,16 @@
|
|||||||
#define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), _cbf);
|
#define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), _cbf);
|
||||||
|
|
||||||
/* Parameter handle datatype */
|
/* Parameter handle datatype */
|
||||||
typedef const char* px4_param_t;
|
typedef const char *px4_param_t;
|
||||||
|
|
||||||
/* Helper fucntions to set ROS params, only int and float supported */
|
/* Helper fucntions to set ROS params, only int and float supported */
|
||||||
static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, int value) {
|
static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, int value)
|
||||||
|
{
|
||||||
ros::param::set(name, value);
|
ros::param::set(name, value);
|
||||||
return (px4_param_t)name;
|
return (px4_param_t)name;
|
||||||
};
|
};
|
||||||
static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, float value) {
|
static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, float value)
|
||||||
|
{
|
||||||
ros::param::set(name, value);
|
ros::param::set(name, value);
|
||||||
return (px4_param_t)name;
|
return (px4_param_t)name;
|
||||||
};
|
};
|
||||||
|
|||||||
@@ -66,7 +66,8 @@ public:
|
|||||||
_pubs()
|
_pubs()
|
||||||
{}
|
{}
|
||||||
|
|
||||||
~NodeHandle() {
|
~NodeHandle()
|
||||||
|
{
|
||||||
//XXX empty lists
|
//XXX empty lists
|
||||||
};
|
};
|
||||||
|
|
||||||
@@ -76,9 +77,10 @@ public:
|
|||||||
* @param fb Callback, executed on receiving a new message
|
* @param fb Callback, executed on receiving a new message
|
||||||
*/
|
*/
|
||||||
template<typename M>
|
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);
|
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);
|
_subs.push_back(sub);
|
||||||
return sub;
|
return sub;
|
||||||
}
|
}
|
||||||
@@ -89,9 +91,10 @@ public:
|
|||||||
* @param fb Callback, executed on receiving a new message
|
* @param fb Callback, executed on receiving a new message
|
||||||
*/
|
*/
|
||||||
template<typename M, typename T>
|
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);
|
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);
|
_subs.push_back(sub);
|
||||||
return sub;
|
return sub;
|
||||||
}
|
}
|
||||||
@@ -101,7 +104,8 @@ public:
|
|||||||
* @param topic Name of the topic
|
* @param topic Name of the topic
|
||||||
*/
|
*/
|
||||||
template<typename M>
|
template<typename M>
|
||||||
Publisher * advertise(const char *topic) {
|
Publisher *advertise(const char *topic)
|
||||||
|
{
|
||||||
ros::Publisher ros_pub = ros::NodeHandle::advertise<M>(topic, kQueueSizeDefault);
|
ros::Publisher ros_pub = ros::NodeHandle::advertise<M>(topic, kQueueSizeDefault);
|
||||||
Publisher *pub = new Publisher(ros_pub);
|
Publisher *pub = new Publisher(ros_pub);
|
||||||
_pubs.push_back(pub);
|
_pubs.push_back(pub);
|
||||||
@@ -121,8 +125,8 @@ public:
|
|||||||
|
|
||||||
private:
|
private:
|
||||||
static const uint32_t kQueueSizeDefault = 1000; /**< Size of queue for ROS */
|
static const uint32_t kQueueSizeDefault = 1000; /**< Size of queue for ROS */
|
||||||
std::list<Subscriber*> _subs; /**< Subcriptions of node */
|
std::list<Subscriber *> _subs; /**< Subcriptions of node */
|
||||||
std::list<Publisher*> _pubs; /**< Publications of node */
|
std::list<Publisher *> _pubs; /**< Publications of node */
|
||||||
};
|
};
|
||||||
#else
|
#else
|
||||||
class __EXPORT NodeHandle
|
class __EXPORT NodeHandle
|
||||||
@@ -144,16 +148,18 @@ public:
|
|||||||
*/
|
*/
|
||||||
|
|
||||||
template<typename M>
|
template<typename M>
|
||||||
Subscriber * subscribe(const struct orb_metadata *meta,
|
Subscriber *subscribe(const struct orb_metadata *meta,
|
||||||
std::function<void(const M&)> callback,
|
std::function<void(const M &)> callback,
|
||||||
unsigned interval) {
|
unsigned interval)
|
||||||
|
{
|
||||||
SubscriberPX4<M> *sub_px4 = new SubscriberPX4<M>(meta, interval, callback, &_subs);
|
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 */
|
/* 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()) {
|
if (_sub_min_interval == nullptr || _sub_min_interval->getInterval() > sub_px4->getInterval()) {
|
||||||
_sub_min_interval = sub_px4;
|
_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
|
* @param meta Describes the topic which is advertised
|
||||||
*/
|
*/
|
||||||
template<typename M>
|
template<typename M>
|
||||||
Publisher * advertise(const struct orb_metadata *meta) {
|
Publisher *advertise(const struct orb_metadata *meta)
|
||||||
|
{
|
||||||
//XXX
|
//XXX
|
||||||
Publisher * pub = new Publisher(meta, &_pubs);
|
Publisher *pub = new Publisher(meta, &_pubs);
|
||||||
return pub;
|
return pub;
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Calls all callback waiting to be called
|
* Calls all callback waiting to be called
|
||||||
*/
|
*/
|
||||||
void spinOnce() {
|
void spinOnce()
|
||||||
|
{
|
||||||
/* Loop through subscriptions, call callback for updated subscriptions */
|
/* Loop through subscriptions, call callback for updated subscriptions */
|
||||||
uORB::SubscriptionNode *sub = _subs.getHead();
|
uORB::SubscriptionNode *sub = _subs.getHead();
|
||||||
int count = 0;
|
int count = 0;
|
||||||
@@ -189,9 +197,11 @@ public:
|
|||||||
/**
|
/**
|
||||||
* Keeps calling callbacks for incomming messages, returns when module is terminated
|
* Keeps calling callbacks for incomming messages, returns when module is terminated
|
||||||
*/
|
*/
|
||||||
void spin() {
|
void spin()
|
||||||
|
{
|
||||||
while (ok()) {
|
while (ok()) {
|
||||||
const int timeout_ms = 100;
|
const int timeout_ms = 100;
|
||||||
|
|
||||||
/* Only continue in the loop if the nodehandle has subscriptions */
|
/* Only continue in the loop if the nodehandle has subscriptions */
|
||||||
if (_sub_min_interval == nullptr) {
|
if (_sub_min_interval == nullptr) {
|
||||||
usleep(timeout_ms * 1000);
|
usleep(timeout_ms * 1000);
|
||||||
@@ -202,6 +212,7 @@ public:
|
|||||||
struct pollfd pfd;
|
struct pollfd pfd;
|
||||||
pfd.fd = _sub_min_interval->getHandle();
|
pfd.fd = _sub_min_interval->getHandle();
|
||||||
pfd.events = POLLIN;
|
pfd.events = POLLIN;
|
||||||
|
|
||||||
if (poll(&pfd, 1, timeout_ms) <= 0) {
|
if (poll(&pfd, 1, timeout_ms) <= 0) {
|
||||||
/* timed out */
|
/* timed out */
|
||||||
continue;
|
continue;
|
||||||
@@ -212,9 +223,9 @@ public:
|
|||||||
}
|
}
|
||||||
private:
|
private:
|
||||||
static const uint16_t kMaxSubscriptions = 100;
|
static const uint16_t kMaxSubscriptions = 100;
|
||||||
List<uORB::SubscriptionNode*> _subs; /**< Subcriptions of node */
|
List<uORB::SubscriptionNode *> _subs; /**< Subcriptions of node */
|
||||||
List<uORB::PublicationNode*> _pubs; /**< Publications of node */
|
List<uORB::PublicationNode *> _pubs; /**< Publications of node */
|
||||||
uORB::SubscriptionNode* _sub_min_interval; /**< Points to the sub wtih the smallest interval
|
uORB::SubscriptionNode *_sub_min_interval; /**< Points to the sub wtih the smallest interval
|
||||||
of all Subscriptions in _subs*/
|
of all Subscriptions in _subs*/
|
||||||
};
|
};
|
||||||
#endif
|
#endif
|
||||||
|
|||||||
@@ -80,7 +80,7 @@ public:
|
|||||||
* @param list publisher is added to this list
|
* @param list publisher is added to this list
|
||||||
*/
|
*/
|
||||||
Publisher(const struct orb_metadata *meta,
|
Publisher(const struct orb_metadata *meta,
|
||||||
List<uORB::PublicationNode *> * list) :
|
List<uORB::PublicationNode *> *list) :
|
||||||
uORB::PublicationNode(meta, list)
|
uORB::PublicationNode(meta, list)
|
||||||
{}
|
{}
|
||||||
|
|
||||||
@@ -90,8 +90,9 @@ public:
|
|||||||
* @param msg the message which is published to the topic
|
* @param msg the message which is published to the topic
|
||||||
*/
|
*/
|
||||||
template<typename M>
|
template<typename M>
|
||||||
int publish(const M &msg) {
|
int publish(const M &msg)
|
||||||
uORB::PublicationBase::update((void*)&msg);
|
{
|
||||||
|
uORB::PublicationBase::update((void *)&msg);
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -97,9 +97,9 @@ public:
|
|||||||
* @param list subscriber is added to this list
|
* @param list subscriber is added to this list
|
||||||
*/
|
*/
|
||||||
SubscriberPX4(const struct orb_metadata *meta,
|
SubscriberPX4(const struct orb_metadata *meta,
|
||||||
unsigned interval,
|
unsigned interval,
|
||||||
std::function<void(const M&)> callback,
|
std::function<void(const M &)> callback,
|
||||||
List<uORB::SubscriptionNode *> * list) :
|
List<uORB::SubscriptionNode *> *list) :
|
||||||
Subscriber(),
|
Subscriber(),
|
||||||
uORB::Subscription<M>(meta, interval, list),
|
uORB::Subscription<M>(meta, interval, list),
|
||||||
_callback(callback)
|
_callback(callback)
|
||||||
@@ -113,7 +113,8 @@ public:
|
|||||||
* Invoked by the list traversal in NodeHandle::spinOnce
|
* Invoked by the list traversal in NodeHandle::spinOnce
|
||||||
* If new data is available the callback is called
|
* If new data is available the callback is called
|
||||||
*/
|
*/
|
||||||
void update() {
|
void update()
|
||||||
|
{
|
||||||
if (!uORB::Subscription<M>::updated()) {
|
if (!uORB::Subscription<M>::updated()) {
|
||||||
/* Topic not updated, do not call callback */
|
/* Topic not updated, do not call callback */
|
||||||
return;
|
return;
|
||||||
@@ -127,7 +128,7 @@ public:
|
|||||||
|
|
||||||
};
|
};
|
||||||
private:
|
private:
|
||||||
std::function<void(const M&)> _callback; /**< Callback handle,
|
std::function<void(const M &)> _callback; /**< Callback handle,
|
||||||
called when new data is available */
|
called when new data is available */
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|||||||
Reference in New Issue
Block a user