From c68c277c94cacd2a64b634dd9c45ace2a04c2911 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 11 Dec 2014 15:04:37 +0100 Subject: subscription class for ros now stores last message to avoid manual copy and support subscription with no callbacks --- src/examples/subscriber/subscriber_example.cpp | 7 +- src/platforms/px4_defines.h | 18 +++-- src/platforms/px4_nodehandle.h | 25 +++--- src/platforms/px4_subscriber.h | 107 +++++++++++++++++++------ 4 files changed, 112 insertions(+), 45 deletions(-) (limited to 'src') diff --git a/src/examples/subscriber/subscriber_example.cpp b/src/examples/subscriber/subscriber_example.cpp index 39d090752..95e208ca6 100644 --- a/src/examples/subscriber/subscriber_example.cpp +++ b/src/examples/subscriber/subscriber_example.cpp @@ -76,8 +76,7 @@ SubscriberExample::SubscriberExample() : * Also the current value of the _sub_rc_chan subscription is printed */ void SubscriberExample::rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg) { - //XXX - // PX4_INFO("Subscriber callback: [%llu], value of _sub_rc_chan: [%llu]", - // msg.timestamp_last_valid, - // ((SubscriberPX4 *)_sub_rc_chan)->timestamp_last_valid); + PX4_INFO("Subscriber callback: [%llu], value of _sub_rc_chan: [%llu]", + msg.timestamp_last_valid, + ((PX4_SUBSCRIBER_T(rc_channels) *)_sub_rc_chan)->get_msg().timestamp_last_valid); } diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h index 819954d0e..fa3ef216a 100644 --- a/src/platforms/px4_defines.h +++ b/src/platforms/px4_defines.h @@ -70,7 +70,7 @@ /* Subscribe and providing a function as callback (do not use directly, use PX4_SUBSCRIBE instead) */ #define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), _cbf); /* Subscribe without a callback (do not use directly, use PX4_SUBSCRIBE instead) */ -#define PX4_SUBSCRIBE_NOCB(_nodehandle, _name, _interval) _nodehandle.subscribe(PX4_TOPIC(_name)); +#define PX4_SUBSCRIBE_NOCB(_nodehandle, _name, _interval) _nodehandle.subscribe(PX4_TOPIC(_name)); /* Parameter handle datatype */ typedef const char *px4_param_t; @@ -93,6 +93,9 @@ static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, float value) /* Get value of parameter */ #define PX4_PARAM_GET(_handle, _destpt) ros::param::get(_handle, *_destpt) +/* Get a subscriber class type based on the topic name */ +#define PX4_SUBSCRIBER_T(_name) SubscriberROS + #else /* * Building for NuttX @@ -130,6 +133,15 @@ typedef param_t px4_param_t; /* Get value of parameter */ #define PX4_PARAM_GET(_handle, _destpt) param_get(_handle, _destpt) + +/* Get a subscriber class type based on the topic name */ +#define PX4_SUBSCRIBER_T(_name) SubscriberUORB + +/* XXX this is a hack to resolve conflicts with NuttX headers */ +#define isspace(c) \ + ((c) == ' ' || (c) == '\t' || (c) == '\n' || \ + (c) == '\r' || (c) == '\f' || c== '\v') + #endif /* Shortcut for subscribing to topics @@ -146,7 +158,3 @@ typedef param_t px4_param_t; /* wrapper for rotation matrices stored in arrays */ #define PX4_R(_array, _x, _y) PX4_ARRAY2D(_array, 3, _x, _y) - -#define isspace(c) \ - ((c) == ' ' || (c) == '\t' || (c) == '\n' || \ - (c) == '\r' || (c) == '\f' || c== '\v') diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index 25b8e037d..c24a18303 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -77,10 +77,11 @@ public: * @param fb Callback, executed on receiving a new message */ template - Subscriber *subscribe(const char *topic, void(*fp)(M)) + Subscriber *subscribe(const char *topic, void(*fp)(const M&)) { - ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, fp); - Subscriber *sub = new Subscriber(ros_sub); + Subscriber *sub = new SubscriberROS(std::bind(fp, std::placeholders::_1)); + ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS::callback, (SubscriberROS*)sub); + ((SubscriberROS*)sub)->set_ros_sub(ros_sub); _subs.push_back(sub); return sub; } @@ -91,10 +92,11 @@ public: * @param fb Callback, executed on receiving a new message */ template - Subscriber *subscribe(const char *topic, void(T::*fp)(M), T *obj) + Subscriber *subscribe(const char *topic, void(T::*fp)(const M&), T *obj) { - ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, fp, obj); - Subscriber *sub = new Subscriber(ros_sub); + Subscriber *sub = new SubscriberROS(std::bind(fp, obj, std::placeholders::_1)); + ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS::callback, (SubscriberROS*)sub); + ((SubscriberROS*)sub)->set_ros_sub(ros_sub); _subs.push_back(sub); return sub; } @@ -106,10 +108,11 @@ public: template Subscriber *subscribe(const char *topic) { - //XXX missing implementation - // Subscriber *sub = new Subscriber(ros_sub); - // _subs.push_back(sub); - return (Subscriber *)NULL; + Subscriber *sub = new SubscriberROS(); + ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS::callback, (SubscriberROS*)sub); + ((SubscriberROS*)sub)->set_ros_sub(ros_sub); + _subs.push_back(sub); + return sub; } /** @@ -165,7 +168,7 @@ public: std::function callback, unsigned interval) { - SubscriberPX4 *sub_px4 = new SubscriberPX4(meta, interval, callback, &_subs); + SubscriberUORB *sub_px4 = new SubscriberUORB(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()) { diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h index 45cb650b2..234b72f5b 100644 --- a/src/platforms/px4_subscriber.h +++ b/src/platforms/px4_subscriber.h @@ -36,6 +36,7 @@ * * PX4 Middleware Wrapper Subscriber */ +#include #pragma once #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) /* includes when building for ros */ @@ -44,59 +45,108 @@ /* includes when building for NuttX */ #include #include -#include +#include "px4_nodehandle.h" #endif namespace px4 { - -#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) +/** + * Subscriber class which is used by nodehandle + */ class Subscriber { public: - /** - * Construct Subscriber by providing a ros::Subscriber - * @param ros_sub the ros subscriber which will be used to perform the publications - */ - Subscriber(ros::Subscriber ros_sub) : - _ros_sub(ros_sub) - {} - + Subscriber() {}; ~Subscriber() {}; -private: - ros::Subscriber _ros_sub; /**< Handle to ros subscriber */ }; -#else - +#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) /** - * Subscriber class which is used by nodehandle when building for NuttX + * Subscriber class that is templated with the ros n message type */ -class Subscriber +template +class SubscriberROS : + public Subscriber { +friend class NodeHandle; + public: - Subscriber() {}; - ~Subscriber() {}; -private: + /** + * Construct Subscriber by providing a callback function + */ + SubscriberROS(std::function cbf) : + Subscriber(), + _ros_sub(), + _cbf(cbf), + _msg_current() + {} + + /** + * Construct Subscriber without a callback function + */ + SubscriberROS() : + Subscriber(), + _ros_sub(), + _cbf(NULL), + _msg_current() + {} + + + ~SubscriberROS() {}; + + /* Accessors*/ + /** + * Get the last message value + */ + const M& get_msg() { return _msg_current; } + +protected: + /** + * Called on topic update, saves the current message and then calls the provided callback function + */ + void callback(const M &msg) { + /* Store data */ + _msg_current = msg; + + /* Call callback */ + if (_cbf != NULL) { + _cbf(msg); + } + + } + + /** + * Saves the ros subscriber to keep ros subscription alive + */ + void set_ros_sub(ros::Subscriber ros_sub) { + _ros_sub = ros_sub; + } + + ros::Subscriber _ros_sub; /**< Handle to ros subscriber */ + std::function _cbf; /**< Callback that the user provided on the subscription */ + M _msg_current; /**< Current Message value */ + }; +#else + /** * Subscriber class that is templated with the uorb subscription message type */ template -class SubscriberPX4 : +class SubscriberUORB : public Subscriber, public uORB::Subscription { public: /** - * Construct SubscriberPX4 by providing orb meta data + * Construct SubscriberUORB by providing orb meta data * @param meta orb metadata for the topic which is used * @param callback Callback, executed on receiving a new message * @param interval Minimal interval between calls to callback * @param list subscriber is added to this list */ - SubscriberPX4(const struct orb_metadata *meta, + SubscriberUORB(const struct orb_metadata *meta, unsigned interval, std::function callback, List *list) : @@ -106,7 +156,7 @@ public: //XXX store callback {} - ~SubscriberPX4() {}; + ~SubscriberUORB() {}; /** * Update Subscription @@ -133,7 +183,14 @@ public: _callback(uORB::Subscription::getData()); }; -private: + + /* Accessors*/ + /** + * Get the last message value + */ + const M& get_msg() { return uORB::Subscription::getData(); } + +protected: std::function _callback; /**< Callback handle, called when new data is available */ -- cgit v1.2.3