From 55cf2fc61c7b90725cd960f9c7d72737024f1cfc Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 25 Nov 2014 11:50:35 +0100 Subject: WIP, towards more px4 compatibility, first macros --- src/examples/publisher/publisher.cpp | 2 +- src/examples/subscriber/subscriber.cpp | 2 +- src/include/px4.h | 5 +++++ src/modules/uORB/Publication.cpp | 1 + src/modules/uORB/Subscription.cpp | 1 + src/platforms/px4_nodehandle.h | 35 +++++++++++++++++++++++++++++++--- src/platforms/px4_publisher.h | 14 +++++++++++++- src/platforms/px4_subscriber.h | 17 +++++++++++++++-- 8 files changed, 69 insertions(+), 8 deletions(-) (limited to 'src') 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("rc_channels"); + px4::Publisher rc_channels_pub = n.advertise(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 + #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; template class __EXPORT Publication; template class __EXPORT Publication; template class __EXPORT Publication; +template class __EXPORT Publication; } 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; template class __EXPORT Subscription; template class __EXPORT Subscription; template class __EXPORT Subscription; +template class __EXPORT Subscription; } // 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 #else /* includes when building for NuttX */ - +#include #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 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 + Subscriber subscribe(const char *topic, void(*fp)(M)) { + Subscriber sub(&_subs, , interval); + return sub; + } + + template + Publisher advertise(const char *topic) { + Publisher pub(ros_pub); + _pubs.push_back(pub); + return pub; + } +private: + List _subs; + List _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 #endif namespace px4 @@ -56,7 +60,15 @@ private: ros::Publisher _ros_pub; }; #else -class Publisher +template +class Publisher : + public uORB::Publication +public: + Publisher(List * 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 #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 +class Subscriber : + public uORB::Subscription +public: + Subscriber(List * list, + const struct orb_metadata *meta, unsigned interval) : + uORB::Subsciption(list, meta, interval) + {} + ~Subscriber() {}; { }; #endif -- cgit v1.2.3