forked from Archive/PX4-Autopilot
ros wrapper: small reordering
This commit is contained in:
parent
978013bbb8
commit
c167df9038
|
@ -37,12 +37,19 @@
|
||||||
* PX4 Middleware Wrapper Node Handle
|
* PX4 Middleware Wrapper Node Handle
|
||||||
*/
|
*/
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
|
/* includes for all platforms */
|
||||||
#include <px4_subscriber.h>
|
#include <px4_subscriber.h>
|
||||||
#include <px4_publisher.h>
|
#include <px4_publisher.h>
|
||||||
|
|
||||||
#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__))
|
#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__))
|
||||||
|
/* includes when building for ros */
|
||||||
#include "ros/ros.h"
|
#include "ros/ros.h"
|
||||||
#include <list>
|
#include <list>
|
||||||
#define QUEUE_SIZE_DEFAULT 1000
|
#include <inttypes.h>
|
||||||
|
#else
|
||||||
|
/* includes when building for NuttX */
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
namespace px4
|
namespace px4
|
||||||
|
@ -59,7 +66,7 @@ public:
|
||||||
|
|
||||||
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, QUEUE_SIZE_DEFAULT, fp);
|
ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, fp);
|
||||||
Subscriber sub(ros_sub);
|
Subscriber sub(ros_sub);
|
||||||
_subs.push_back(sub);
|
_subs.push_back(sub);
|
||||||
return sub;
|
return sub;
|
||||||
|
@ -67,12 +74,13 @@ public:
|
||||||
|
|
||||||
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, QUEUE_SIZE_DEFAULT);
|
ros::Publisher ros_pub = ros::NodeHandle::advertise<M>(topic, kQueueSizeDefault);
|
||||||
Publisher pub(ros_pub);
|
Publisher pub(ros_pub);
|
||||||
_pubs.push_back(pub);
|
_pubs.push_back(pub);
|
||||||
return pub;
|
return pub;
|
||||||
}
|
}
|
||||||
private:
|
private:
|
||||||
|
static const uint32_t kQueueSizeDefault = 1000;
|
||||||
std::list<Subscriber> _subs;
|
std::list<Subscriber> _subs;
|
||||||
std::list<Publisher> _pubs;
|
std::list<Publisher> _pubs;
|
||||||
};
|
};
|
||||||
|
|
|
@ -46,14 +46,14 @@ namespace px4
|
||||||
#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__))
|
#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__))
|
||||||
class Publisher
|
class Publisher
|
||||||
{
|
{
|
||||||
private:
|
|
||||||
ros::Publisher _ros_pub;
|
|
||||||
public:
|
public:
|
||||||
Publisher(ros::Publisher ros_pub) : _ros_pub(ros_pub)
|
Publisher(ros::Publisher ros_pub) : _ros_pub(ros_pub)
|
||||||
{}
|
{}
|
||||||
~Publisher() {};
|
~Publisher() {};
|
||||||
template<typename M>
|
template<typename M>
|
||||||
int publish(const M &msg) { _ros_pub.publish(msg); return 0; }
|
int publish(const M &msg) { _ros_pub.publish(msg); return 0; }
|
||||||
|
private:
|
||||||
|
ros::Publisher _ros_pub;
|
||||||
};
|
};
|
||||||
#else
|
#else
|
||||||
class Publisher
|
class Publisher
|
||||||
|
|
|
@ -47,12 +47,12 @@ namespace px4
|
||||||
#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__))
|
#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__))
|
||||||
class Subscriber
|
class Subscriber
|
||||||
{
|
{
|
||||||
private:
|
|
||||||
ros::Subscriber _ros_sub;
|
|
||||||
public:
|
public:
|
||||||
Subscriber(ros::Subscriber ros_sub) : _ros_sub(ros_sub)
|
Subscriber(ros::Subscriber ros_sub) : _ros_sub(ros_sub)
|
||||||
{}
|
{}
|
||||||
~Subscriber() {};
|
~Subscriber() {};
|
||||||
|
private:
|
||||||
|
ros::Subscriber _ros_sub;
|
||||||
};
|
};
|
||||||
#else
|
#else
|
||||||
class Subscriber
|
class Subscriber
|
||||||
|
|
Loading…
Reference in New Issue