diff options
-rw-r--r-- | src/examples/publisher/publisher.cpp | 2 | ||||
-rw-r--r-- | src/examples/subscriber/subscriber.cpp | 2 | ||||
-rw-r--r-- | src/include/px4.h | 5 | ||||
-rw-r--r-- | src/modules/uORB/Publication.cpp | 1 | ||||
-rw-r--r-- | src/modules/uORB/Subscription.cpp | 1 | ||||
-rw-r--r-- | src/platforms/px4_nodehandle.h | 35 | ||||
-rw-r--r-- | src/platforms/px4_publisher.h | 14 | ||||
-rw-r--r-- | src/platforms/px4_subscriber.h | 17 |
8 files changed, 69 insertions, 8 deletions
diff --git a/src/examples/publisher/publisher.cpp b/src/examples/publisher/publisher.cpp index 6869e765b..c09cca1fc 100644 --- a/src/examples/publisher/publisher.cpp +++ b/src/examples/publisher/publisher.cpp @@ -57,7 +57,7 @@ int main(int argc, char **argv) * than we can send them, the number here specifies how many messages to * buffer up before throwing some away. */ - px4::Publisher rc_channels_pub = n.advertise<px4::rc_channels>("rc_channels"); + px4::Publisher rc_channels_pub = n.advertise<px4::rc_channels>(PX4_TOPIC(rc_channels)); px4::Rate loop_rate(10); diff --git a/src/examples/subscriber/subscriber.cpp b/src/examples/subscriber/subscriber.cpp index 828b1f937..b91859027 100644 --- a/src/examples/subscriber/subscriber.cpp +++ b/src/examples/subscriber/subscriber.cpp @@ -74,7 +74,7 @@ int main(int argc, char **argv) * is the number of messages that will be buffered up before beginning to throw * away the oldest ones. */ - px4::Subscriber sub = n.subscribe("rc_channels", rc_channels_callback); + px4::Subscriber sub = n.subscribe(PX4_TOPIC(rc_channels), rc_channels_callback); PX4_INFO("subscribed"); /** diff --git a/src/include/px4.h b/src/include/px4.h index bb97f2a8a..391972b12 100644 --- a/src/include/px4.h +++ b/src/include/px4.h @@ -46,14 +46,19 @@ * Building for running within the ROS environment */ #include "ros/ros.h" + #define PX4_WARN ROS_WARN #define PX4_INFO ROS_INFO +#define PX4_TOPIC(name) #name #else /* * Building for NuttX */ +#include <uORB/uORB.h> + #define PX4_WARN warnx #define PX4_INFO warnx +#define PX4_TOPIC(name) ORB_ID(name) #endif #include "../platforms/px4_defines.h" diff --git a/src/modules/uORB/Publication.cpp b/src/modules/uORB/Publication.cpp index cd0b30dd6..302b99c69 100644 --- a/src/modules/uORB/Publication.cpp +++ b/src/modules/uORB/Publication.cpp @@ -78,5 +78,6 @@ template class __EXPORT Publication<vehicle_rates_setpoint_s>; template class __EXPORT Publication<actuator_outputs_s>; template class __EXPORT Publication<encoders_s>; template class __EXPORT Publication<tecs_status_s>; +template class __EXPORT Publication<rc_channels_s>; } diff --git a/src/modules/uORB/Subscription.cpp b/src/modules/uORB/Subscription.cpp index 44b6debc7..3406a66d3 100644 --- a/src/modules/uORB/Subscription.cpp +++ b/src/modules/uORB/Subscription.cpp @@ -101,5 +101,6 @@ template class __EXPORT Subscription<vehicle_local_position_setpoint_s>; template class __EXPORT Subscription<vehicle_local_position_s>; template class __EXPORT Subscription<vehicle_attitude_setpoint_s>; template class __EXPORT Subscription<vehicle_rates_setpoint_s>; +template class __EXPORT Subscription<rc_channels_s>; } // namespace uORB diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index d66884445..fa2c8d6a4 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -49,21 +49,26 @@ #include <inttypes.h> #else /* includes when building for NuttX */ - +#include <containers/List.hpp> #endif namespace px4 { #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) -class NodeHandle : private ros::NodeHandle +class NodeHandle : + private ros::NodeHandle { public: - NodeHandle () : + NodeHandle() : ros::NodeHandle(), _subs(), _pubs() {} + ~NodeHandle() { + //XXX empty lists + }; + template<typename M> Subscriber subscribe(const char *topic, void(*fp)(M)) { ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, fp); @@ -87,6 +92,30 @@ private: #else class NodeHandle { +public: + NodeHandle() : + _subs(), + _pubs() + {} + + ~NodeHandle() {}; + + template<typename M> + Subscriber subscribe(const char *topic, void(*fp)(M)) { + Subscriber sub(&_subs, , interval); + return sub; + } + + template<typename M> + Publisher advertise(const char *topic) { + Publisher pub(ros_pub); + _pubs.push_back(pub); + return pub; + } +private: + List<Subscriber> _subs; + List<Publisher> _pubs; + }; #endif } diff --git a/src/platforms/px4_publisher.h b/src/platforms/px4_publisher.h index 72f69a5af..53e63b695 100644 --- a/src/platforms/px4_publisher.h +++ b/src/platforms/px4_publisher.h @@ -38,7 +38,11 @@ */ #pragma once #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) +/* includes when building for ros */ #include "ros/ros.h" +#else +/* includes when building for NuttX */ +#include <uORB/Publication.hpp> #endif namespace px4 @@ -56,7 +60,15 @@ private: ros::Publisher _ros_pub; }; #else -class Publisher +template<typename M> +class Publisher : + public uORB::Publication<M> +public: + Publisher(List<SubscriptionBase *> * list, + const struct orb_metadata *meta, unsigned interval) : + uORB::Publication(list, meta) + {} + ~Publisher() {}; { }; #endif diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h index 78be3ebff..6312e0cbe 100644 --- a/src/platforms/px4_subscriber.h +++ b/src/platforms/px4_subscriber.h @@ -38,7 +38,11 @@ */ #pragma once #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) +/* includes when building for ros */ #include "ros/ros.h" +#else +/* includes when building for NuttX */ +#include <uORB/Subscription.hpp> #endif namespace px4 @@ -48,14 +52,23 @@ namespace px4 class Subscriber { public: - Subscriber(ros::Subscriber ros_sub) : _ros_sub(ros_sub) + Subscriber(ros::Subscriber ros_sub) : + _ros_sub(ros_sub) {} ~Subscriber() {}; private: ros::Subscriber _ros_sub; }; #else -class Subscriber +template<typename M> +class Subscriber : + public uORB::Subscription<M> +public: + Subscriber(List<SubscriptionBase *> * list, + const struct orb_metadata *meta, unsigned interval) : + uORB::Subsciption(list, meta, interval) + {} + ~Subscriber() {}; { }; #endif |