diff options
Diffstat (limited to 'src')
-rw-r--r-- | src/examples/publisher/publisher_example.cpp | 19 | ||||
-rw-r--r-- | src/examples/publisher/publisher_example.h | 1 | ||||
-rw-r--r-- | src/examples/subscriber/subscriber_example.cpp | 14 | ||||
-rw-r--r-- | src/examples/subscriber/subscriber_example.h | 3 | ||||
-rw-r--r-- | src/platforms/nuttx/px4_messages/px4_rc_channels.h | 26 | ||||
-rw-r--r-- | src/platforms/px4_includes.h | 4 | ||||
-rw-r--r-- | src/platforms/px4_message.h | 77 | ||||
-rw-r--r-- | src/platforms/px4_nodehandle.h | 180 | ||||
-rw-r--r-- | src/platforms/px4_publisher.h | 41 | ||||
-rw-r--r-- | src/platforms/px4_subscriber.h | 114 | ||||
-rw-r--r-- | src/platforms/ros/px4_messages/px4_rc_channels.h | 25 |
11 files changed, 388 insertions, 116 deletions
diff --git a/src/examples/publisher/publisher_example.cpp b/src/examples/publisher/publisher_example.cpp index 2e5779ebe..e85e42b38 100644 --- a/src/examples/publisher/publisher_example.cpp +++ b/src/examples/publisher/publisher_example.cpp @@ -45,7 +45,8 @@ using namespace px4; PublisherExample::PublisherExample() : _n(), - _rc_channels_pub(PX4_ADVERTISE(_n, rc_channels)) + _rc_channels_pub(_n.advertise<px4_rc_channels>()) + // _rc_channels_pub(PX4_ADVERTISE(_n, rc_channels)) { } @@ -55,11 +56,19 @@ int PublisherExample::main() px4::Rate loop_rate(10); while (px4::ok()) { - PX4_TOPIC_T(rc_channels) msg; - msg.timestamp_last_valid = px4::get_time_micros(); - PX4_INFO("%llu", msg.timestamp_last_valid); + // PX4_TOPIC_T(rc_channels) msg; + // msg.timestamp_last_valid = px4::get_time_micros(); + // PX4_INFO("%llu", msg.timestamp_last_valid); + + // _rc_channels_pub->publish(msg); + + //XXX + px4_rc_channels msg2; + msg2.data().timestamp_last_valid = px4::get_time_micros(); + PX4_INFO("%llu", msg2.data().timestamp_last_valid); + // msg2.pub->publish2(); + _rc_channels_pub->publish(msg2); - _rc_channels_pub->publish(msg); _n.spinOnce(); loop_rate.sleep(); diff --git a/src/examples/publisher/publisher_example.h b/src/examples/publisher/publisher_example.h index 304ecef47..86586d0d3 100644 --- a/src/examples/publisher/publisher_example.h +++ b/src/examples/publisher/publisher_example.h @@ -48,5 +48,6 @@ public: int main(); protected: px4::NodeHandle _n; + // px4::Publisher<px4::px4_rc_channels> * _rc_channels_pub; px4::Publisher * _rc_channels_pub; }; diff --git a/src/examples/subscriber/subscriber_example.cpp b/src/examples/subscriber/subscriber_example.cpp index 3c80561ca..1c8f4c62b 100644 --- a/src/examples/subscriber/subscriber_example.cpp +++ b/src/examples/subscriber/subscriber_example.cpp @@ -62,11 +62,13 @@ SubscriberExample::SubscriberExample() : /* Do some subscriptions */ /* Function */ - PX4_SUBSCRIBE(_n, rc_channels, rc_channels_callback_function, _interval); - /* Class Method */ - PX4_SUBSCRIBE(_n, rc_channels, SubscriberExample::rc_channels_callback, this, 1000); - /* No callback */ - _sub_rc_chan = PX4_SUBSCRIBE(_n, rc_channels, 500); + // PX4_SUBSCRIBE(_n, rc_channels, rc_channels_callback_function, _interval); + _n.subscribe<px4_rc_channels>(rc_channels_callback_function); + + // [> Class Method <] + // PX4_SUBSCRIBE(_n, rc_channels, SubscriberExample::rc_channels_callback, this, 1000); + // [> No callback <] + // _sub_rc_chan = PX4_SUBSCRIBE(_n, rc_channels, 500); PX4_INFO("subscribed"); } @@ -78,5 +80,5 @@ SubscriberExample::SubscriberExample() : void SubscriberExample::rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg) { PX4_INFO("Subscriber callback: [%llu], value of _sub_rc_chan: [%llu]", msg.timestamp_last_valid, - _sub_rc_chan->get().timestamp_last_valid); + _sub_rc_chan->get().data().timestamp_last_valid); } diff --git a/src/examples/subscriber/subscriber_example.h b/src/examples/subscriber/subscriber_example.h index eb2c956e0..b52ae991b 100644 --- a/src/examples/subscriber/subscriber_example.h +++ b/src/examples/subscriber/subscriber_example.h @@ -56,7 +56,8 @@ protected: int32_t _interval; px4_param_t _p_test_float; float _test_float; - px4::PX4_SUBSCRIBER(rc_channels) * _sub_rc_chan; + // px4::PX4_SUBSCRIBER(rc_channels) * _sub_rc_chan; + px4::Subscriber<px4_rc_channels> * _sub_rc_chan; void rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg); diff --git a/src/platforms/nuttx/px4_messages/px4_rc_channels.h b/src/platforms/nuttx/px4_messages/px4_rc_channels.h new file mode 100644 index 000000000..bfca48469 --- /dev/null +++ b/src/platforms/nuttx/px4_messages/px4_rc_channels.h @@ -0,0 +1,26 @@ +#include <uORB/uORB.h> +#include <uORB/topics/rc_channels.h> +#include "platforms/px4_message.h" + +#pragma once +namespace px4 +{ + +class px4_rc_channels : + public PX4Message<rc_channels_s> +{ +public: + px4_rc_channels() : + PX4Message<rc_channels_s>() + {} + + px4_rc_channels(rc_channels_s msg) : + PX4Message<rc_channels_s>(msg) + {} + + ~px4_rc_channels() {} + + PX4TopicHandle handle() {return (PX4TopicHandle)ORB_ID(rc_channels);} +}; + +} diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h index fc31162c8..bdb2b8de9 100644 --- a/src/platforms/px4_includes.h +++ b/src/platforms/px4_includes.h @@ -49,6 +49,7 @@ #ifdef __cplusplus #include "ros/ros.h" #include "px4/rc_channels.h" +#include "px4_rc_channels.h" //XXX #include "px4/vehicle_attitude.h" #include <px4/vehicle_attitude_setpoint.h> #include <px4/manual_control_setpoint.h> @@ -71,6 +72,9 @@ #include <nuttx/config.h> #include <uORB/uORB.h> #include <uORB/topics/rc_channels.h> +#ifdef __cplusplus +#include <platforms/nuttx/px4_messages/px4_rc_channels.h> //XXX +#endif #include <uORB/topics/vehicle_attitude_setpoint.h> #include <uORB/topics/manual_control_setpoint.h> #include <uORB/topics/actuator_controls.h> diff --git a/src/platforms/px4_message.h b/src/platforms/px4_message.h new file mode 100644 index 000000000..05fcf1140 --- /dev/null +++ b/src/platforms/px4_message.h @@ -0,0 +1,77 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file px4_message.h + * + * Defines the message base types + */ +#pragma once + +#if defined(__PX4_ROS) +typedef const char* PX4TopicHandle; +#else +#include <uORB/uORB.h> +typedef const struct orb_metatdata* PX4TopicHandle; +#endif + +namespace px4 +{ + +template <typename M> +class PX4Message +{ + // friend class NodeHandle; +// #if defined(__PX4_ROS) + // template<typename T> + // friend class SubscriberROS; +// #endif + +public: + PX4Message() : + _data() + {} + + PX4Message(M msg) : + _data(msg) + {} + + virtual ~PX4Message() {}; + + virtual M& data() {return _data;} + virtual PX4TopicHandle handle() = 0; +private: + M _data; +}; + +} diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index 7b14caed9..a40581239 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -48,6 +48,7 @@ #include "ros/ros.h" #include <list> #include <inttypes.h> +#include <type_traits> #else /* includes when building for NuttX */ #include <poll.h> @@ -77,15 +78,25 @@ public: * @param topic Name of the topic * @param fb Callback, executed on receiving a new message */ - template<typename M> - Subscriber<M> *subscribe(const char *topic, void(*fp)(const M &)) + // template<typename M> + // Subscriber<M> *subscribe(const char *topic, void(*fp)(const M &)) + // { + // SubscriberBase *sub = new SubscriberROS<M>(std::bind(fp, std::placeholders::_1)); + // ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS<M>::callback, + // (SubscriberROS<M> *)sub); + // ((SubscriberROS<M> *)sub)->set_ros_sub(ros_sub); + // _subs.push_back(sub); + // return (Subscriber<M> *)sub; + // } + template<typename T> + Subscriber<T> *subscribe(void(*fp)(const typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &)) { - SubscriberBase *sub = new SubscriberROS<M>(std::bind(fp, std::placeholders::_1)); - ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS<M>::callback, - (SubscriberROS<M> *)sub); - ((SubscriberROS<M> *)sub)->set_ros_sub(ros_sub); + SubscriberBase *sub = new SubscriberROS<T>(std::bind(fp, std::placeholders::_1)); + ros::Subscriber ros_sub = ros::NodeHandle::subscribe((new T())->handle(), kQueueSizeDefault, + &SubscriberROS<T>::callback, (SubscriberROS<T> *)sub); + ((SubscriberROS<T> *)sub)->set_ros_sub(ros_sub); _subs.push_back(sub); - return (Subscriber<M> *)sub; + return (Subscriber<T> *)sub; } /** @@ -93,40 +104,49 @@ public: * @param topic Name of the topic * @param fb Callback, executed on receiving a new message */ - template<typename M, typename T> - Subscriber<M> *subscribe(const char *topic, void(T::*fp)(const M &), T *obj) - { - SubscriberBase *sub = new SubscriberROS<M>(std::bind(fp, obj, std::placeholders::_1)); - ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS<M>::callback, - (SubscriberROS<M> *)sub); - ((SubscriberROS<M> *)sub)->set_ros_sub(ros_sub); - _subs.push_back(sub); - return (Subscriber<M> *)sub; - } + // template<typename M, typename T> + // Subscriber<M> *subscribe(const char *topic, void(T::*fp)(const M &), T *obj) + // { + // SubscriberBase *sub = new SubscriberROS<M>(std::bind(fp, obj, std::placeholders::_1)); + // ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS<M>::callback, + // (SubscriberROS<M> *)sub); + // ((SubscriberROS<M> *)sub)->set_ros_sub(ros_sub); + // _subs.push_back(sub); + // return (Subscriber<M> *)sub; + // } /** * Subscribe with no callback, just the latest value is stored on updates * @param topic Name of the topic */ - template<typename M> - Subscriber<M> *subscribe(const char *topic) - { - SubscriberBase *sub = new SubscriberROS<M>(); - ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS<M>::callback, - (SubscriberROS<M> *)sub); - ((SubscriberROS<M> *)sub)->set_ros_sub(ros_sub); - _subs.push_back(sub); - return (Subscriber<M> *)sub; - } + // template<typename M> + // Subscriber<M> *subscribe(const char *topic) + // { + // SubscriberBase *sub = new SubscriberROS<M>(); + // ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS<M>::callback, + // (SubscriberROS<M> *)sub); + // ((SubscriberROS<M> *)sub)->set_ros_sub(ros_sub); + // _subs.push_back(sub); + // return (Subscriber<M> *)sub; + // } /** * Advertise topic * @param topic Name of the topic */ - template<typename M> - Publisher *advertise(const char *topic) + // template<typename M> + // Publisher *advertise(const char *topic) + // { + // ros::Publisher ros_pub = ros::NodeHandle::advertise<M>(topic, kQueueSizeDefault); + // Publisher *pub = new Publisher(ros_pub); + // _pubs.push_back(pub); + // return pub; + // } + template<typename T> + Publisher* advertise() + // Publisher<T> *advertise() { - ros::Publisher ros_pub = ros::NodeHandle::advertise<M>(topic, kQueueSizeDefault); + ros::Publisher ros_pub = ros::NodeHandle::advertise<typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &>((new T())->handle(), kQueueSizeDefault); Publisher *pub = new Publisher(ros_pub); _pubs.push_back(pub); return pub; @@ -161,7 +181,7 @@ public: ~NodeHandle() { /* Empty subscriptions list */ - uORB::SubscriptionNode *sub = _subs.getHead(); + SubscriberNode *sub = _subs.getHead(); int count = 0; while (sub != nullptr) { @@ -170,13 +190,13 @@ public: break; } - uORB::SubscriptionNode *sib = sub->getSibling(); + SubscriberNode *sib = sub->getSibling(); delete sub; sub = sib; } /* Empty publications list */ - uORB::PublicationNode *pub = _pubs.getHead(); + Publisher *pub = _pubs.getHead(); count = 0; while (pub != nullptr) { @@ -185,7 +205,7 @@ public: break; } - uORB::PublicationNode *sib = pub->getSibling(); + Publisher *sib = pub->getSibling(); delete pub; pub = sib; } @@ -198,19 +218,41 @@ public: * @param interval Minimal interval between calls to callback */ - template<typename M> - Subscriber<M> *subscribe(const struct orb_metadata *meta, - std::function<void(const M &)> callback, - unsigned interval) + // template<typename M> + // Subscriber<M> *subscribe(const struct orb_metadata *meta, + // std::function<void(const M &)> callback, + // unsigned interval) + // { + // SubscriberUORBCallback<M> *sub_px4 = new SubscriberUORBCallback<M>(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()) { + // _sub_min_interval = sub_px4; + // } + + // return (Subscriber<M> *)sub_px4; + // } + /** + * Subscribe with callback to function + * @param meta Describes the topic which nodehande should subscribe to + * @param callback Callback, executed on receiving a new message + * @param interval Minimal interval between calls to callback + */ + + template<typename T> + Subscriber<T> *subscribe(std::function<void(const typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &)> callback, unsigned interval=10) //XXX interval { - SubscriberUORBCallback<M> *sub_px4 = new SubscriberUORBCallback<M>(meta, interval, callback, &_subs); + const struct orb_metadata * meta = NULL; + uORB::SubscriptionBase * uorb_sub = new uORB::SubscriptionBase(meta, interval); + SubscriberUORBCallback<T> *sub_px4 = new SubscriberUORBCallback<T>(uorb_sub, callback); /* 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->get_interval() > interval) { _sub_min_interval = sub_px4; } + _subs.add((SubscriberNode *)sub_px4); - return (Subscriber<M> *)sub_px4; + return (Subscriber<T> *)sub_px4; } /** @@ -219,29 +261,47 @@ public: * @param interval Minimal interval between data fetches from orb */ - template<typename M> - Subscriber<M> *subscribe(const struct orb_metadata *meta, - unsigned interval) - { - SubscriberUORB<M> *sub_px4 = new SubscriberUORB<M>(meta, interval, &_subs); + // template<typename M> + // Subscriber<M> *subscribe(const struct orb_metadata *meta, + // unsigned interval) + // { + // SubscriberUORB<M> *sub_px4 = new SubscriberUORB<M>(meta, interval, &_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()) { - _sub_min_interval = sub_px4; - } + // [> 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()) { + // _sub_min_interval = sub_px4; + // } - return (Subscriber<M> *)sub_px4; - } + // return (Subscriber<M> *)sub_px4; + // } /** * Advertise topic * @param meta Describes the topic which is advertised */ - template<typename M> - Publisher *advertise(const struct orb_metadata *meta) + // template<typename M> + // Publisher *advertise(const struct orb_metadata *meta) + // { + // //XXX + // Publisher *pub = new Publisher(meta, &_pubs); + // return pub; + // } + + /** + * Advertise topic + * @param meta Describes the topic which is advertised + */ + template<typename T> + Publisher *advertise() { //XXX - Publisher *pub = new Publisher(meta, &_pubs); + // uORB::PublicationBase * uorb_pub = new uORB::PublicationBase((new T())->handle()); + const struct orb_metadata * meta = NULL; + uORB::PublicationBase * uorb_pub = new uORB::PublicationBase(meta); + Publisher *pub = new Publisher(uorb_pub); + + _pubs.add(pub); + return pub; } @@ -251,7 +311,7 @@ public: void spinOnce() { /* Loop through subscriptions, call callback for updated subscriptions */ - uORB::SubscriptionNode *sub = _subs.getHead(); + SubscriberNode *sub = _subs.getHead(); int count = 0; while (sub != nullptr) { @@ -281,7 +341,7 @@ public: /* Poll fd with smallest interval */ struct pollfd pfd; - pfd.fd = _sub_min_interval->getHandle(); + pfd.fd = _sub_min_interval->getUORBHandle(); pfd.events = POLLIN; poll(&pfd, 1, timeout_ms); spinOnce(); @@ -290,9 +350,9 @@ public: private: static const uint16_t kMaxSubscriptions = 100; static const uint16_t kMaxPublications = 100; - List<uORB::SubscriptionNode *> _subs; /**< Subcriptions of node */ - List<uORB::PublicationNode *> _pubs; /**< Publications of node */ - uORB::SubscriptionNode *_sub_min_interval; /**< Points to the sub wtih the smallest interval + List<SubscriberNode *> _subs; /**< Subcriptions of node */ + List<Publisher *> _pubs; /**< Publications of node */ + SubscriberNode *_sub_min_interval; /**< Points to the sub wtih the smallest interval of all Subscriptions in _subs*/ }; #endif diff --git a/src/platforms/px4_publisher.h b/src/platforms/px4_publisher.h index c6f3d6108..aff045d23 100644 --- a/src/platforms/px4_publisher.h +++ b/src/platforms/px4_publisher.h @@ -46,6 +46,8 @@ #include <containers/List.hpp> #endif +#include <platforms/px4_message.h> + namespace px4 { @@ -58,11 +60,19 @@ public: PublisherBase() {}; ~PublisherBase() {}; + /** Publishes msg + * @param msg the message which is published to the topic + */ + // virtual int publish2(const PX4Message * const msg) = 0; + }; #if defined(__PX4_ROS) +// template <typename T> class Publisher : public PublisherBase + // public PublisherBase, + // public T { public: /** @@ -71,6 +81,7 @@ public: */ Publisher(ros::Publisher ros_pub) : PublisherBase(), + // T(), _ros_pub(ros_pub) {} @@ -79,15 +90,20 @@ public: /** Publishes msg * @param msg the message which is published to the topic */ - 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; } + template <typename T> + int publish(T &msg) { + _ros_pub.publish(msg.data()); + return 0;} private: ros::Publisher _ros_pub; /**< Handle to the ros publisher */ }; #else class Publisher : - public uORB::PublicationNode, - public PublisherBase + // public uORB::PublicationNode, + public PublisherBase, + public ListNode<Publisher *> + { public: /** @@ -95,10 +111,14 @@ public: * @param meta orb metadata for the topic which is used * @param list publisher is added to this list */ - Publisher(const struct orb_metadata *meta, - List<uORB::PublicationNode *> *list) : - uORB::PublicationNode(meta, list), - PublisherBase() + // Publisher(const struct orb_metadata *meta, + // List<uORB::PublicationNode *> *list) : + // uORB::PublicationNode(meta, list), + // PublisherBase() + // {} + Publisher(uORB::PublicationBase * uorb_pub) : + PublisherBase(), + _uorb_pub(uorb_pub) {} ~Publisher() {}; @@ -109,7 +129,7 @@ public: template<typename M> int publish(const M &msg) { - uORB::PublicationBase::update((void *)&msg); + _uorb_pub->update((void *)&msg); return 0; } @@ -117,6 +137,9 @@ public: * Empty callback for list traversal */ void update() {} ; +private: + uORB::PublicationBase * _uorb_pub; /**< Handle to the publisher */ + }; #endif } diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h index aaacf9e48..ef03922ad 100644 --- a/src/platforms/px4_subscriber.h +++ b/src/platforms/px4_subscriber.h @@ -39,6 +39,7 @@ #pragma once #include <functional> +#include <type_traits> #if defined(__PX4_ROS) /* includes when building for ros */ @@ -67,7 +68,7 @@ public: /** * Subscriber class which is used by nodehandle */ -template<typename M> +template<typename T> class Subscriber : public SubscriberBase { @@ -81,7 +82,7 @@ public: /** * Get the last message value */ - virtual M get() = 0; + virtual T get() = 0; /** * Get void pointer to last message value @@ -93,9 +94,9 @@ public: /** * Subscriber class that is templated with the ros n message type */ -template<typename M> +template<typename T> class SubscriberROS : - public Subscriber<M> + public Subscriber<T> { friend class NodeHandle; @@ -103,8 +104,8 @@ public: /** * Construct Subscriber by providing a callback function */ - SubscriberROS(std::function<void(const M &)> cbf) : - Subscriber<M>(), + SubscriberROS(std::function<void(const typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &)> cbf) : + Subscriber<T>(), _ros_sub(), _cbf(cbf), _msg_current() @@ -114,7 +115,7 @@ public: * Construct Subscriber without a callback function */ SubscriberROS() : - Subscriber<M>(), + Subscriber<T>(), _ros_sub(), _cbf(NULL), _msg_current() @@ -127,7 +128,7 @@ public: /** * Get the last message value */ - M get() { return _msg_current; } + T get() { return _msg_current; } /** * Get void pointer to last message value */ @@ -137,10 +138,10 @@ protected: /** * Called on topic update, saves the current message and then calls the provided callback function */ - void callback(const M &msg) + void callback(const typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &msg) { /* Store data */ - _msg_current = msg; + _msg_current = (T)msg; /* Call callback */ if (_cbf != NULL) { @@ -158,20 +159,47 @@ protected: } ros::Subscriber _ros_sub; /**< Handle to ros subscriber */ - std::function<void(const M &)> _cbf; /**< Callback that the user provided on the subscription */ - M _msg_current; /**< Current Message value */ + std::function<void(const typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &)> _cbf; /**< Callback that the user provided on the subscription */ + T _msg_current; /**< Current Message value */ }; #else // Building for NuttX + +/** + * Because we maintain a list of subscribers we need a node class + */ +class SubscriberNode : + public ListNode<SubscriberNode *> +{ +public: + SubscriberNode(unsigned interval) : + ListNode(), + _interval(interval) + {} + + virtual ~SubscriberNode() {} + + virtual void update() = 0; + + virtual int getUORBHandle() = 0; + + unsigned get_interval() { return _interval; } + +protected: + unsigned _interval; + +}; + + /** * Subscriber class that is templated with the uorb subscription message type */ -template<typename M> +template<typename T> class SubscriberUORB : - public Subscriber<M>, - public uORB::Subscription<M> + public Subscriber<T>, + public SubscriberNode { public: @@ -181,11 +209,15 @@ public: * @param interval Minimal interval between calls to callback * @param list subscriber is added to this list */ - SubscriberUORB(const struct orb_metadata *meta, - unsigned interval, - List<uORB::SubscriptionNode *> *list) : - Subscriber<M>(), - uORB::Subscription<M>(meta, interval, list) + // SubscriberUORB(const struct orb_metadata *meta, + // unsigned interval, + // List<uORB::SubscriptionNode *> *list) : + // Subscriber<M>(), + // uORB::Subscription<M>(meta, interval, list) + // {} + SubscriberUORB(uORB::SubscriptionBase * uorb_sub, unsigned interval) : + SubscriberNode(interval), + _uorb_sub(uorb_sub) {} ~SubscriberUORB() {}; @@ -196,29 +228,36 @@ public: */ virtual void update() { - if (!uORB::Subscription<M>::updated()) { + if (!_uorb_sub->updated()) { /* Topic not updated */ return; } /* get latest data */ - uORB::Subscription<M>::update(); + _uorb_sub->update(get_void_ptr()); }; /* Accessors*/ /** * Get the last message value */ - M get() { return uORB::Subscription<M>::getData(); } + T get() { return (T)(typename std::remove_reference<decltype(((T*)nullptr)->data())>::type)*_uorb_sub; } /** * Get void pointer to last message value */ - void *get_void_ptr() { return uORB::Subscription<M>::getDataVoidPtr(); } + void *get_void_ptr() { return (void *)(typename std::remove_reference<decltype(((T*)nullptr)->data())>::type*)_uorb_sub; } + + int getUORBHandle() { return _uorb_sub->getHandle(); } + +protected: + uORB::SubscriptionBase * _uorb_sub; /**< Handle to the subscription */ + typename std::remove_reference<decltype(((T*)nullptr)->data())>::type getUORBData() { return (typename std::remove_reference<decltype(((T*)nullptr)->data())>::type)*_uorb_sub; } }; -template<typename M> +//XXX reduce to one class with overloaded constructor? +template<typename T> class SubscriberUORBCallback : - public SubscriberUORB<M> + public SubscriberUORB<T> { public: /** @@ -228,11 +267,16 @@ public: * @param interval Minimal interval between calls to callback * @param list subscriber is added to this list */ - SubscriberUORBCallback(const struct orb_metadata *meta, - unsigned interval, - std::function<void(const M &)> callback, - List<uORB::SubscriptionNode *> *list) : - SubscriberUORB<M>(meta, interval, list), + // SubscriberUORBCallback(const struct orb_metadata *meta, + // unsigned interval, + // std::function<void(const M &)> callback, + // List<uORB::SubscriptionNode *> *list) : + // SubscriberUORB<M>(meta, interval, list), + // _callback(callback) + // {} + SubscriberUORBCallback(uORB::SubscriptionBase * uorb_sub, + std::function<void(typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &)> callback) : + SubscriberUORB<T>(uorb_sub), _callback(callback) {} @@ -245,13 +289,13 @@ public: */ virtual void update() { - if (!uORB::Subscription<M>::updated()) { + if (!SubscriberUORB<T>::_uorb_sub->updated()) { /* Topic not updated, do not call callback */ return; } /* get latest data */ - uORB::Subscription<M>::update(); + SubscriberUORB<T>::_uorb_sub->update(); /* Check if there is a callback */ @@ -260,12 +304,12 @@ public: } /* Call callback which performs actions based on this data */ - _callback(uORB::Subscription<M>::getData()); + _callback(SubscriberUORB<T>::getUORBData()); }; protected: - std::function<void(const M &)> _callback; /**< Callback handle, + std::function<void(const typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &)> _callback; /**< Callback handle, called when new data is available */ }; #endif diff --git a/src/platforms/ros/px4_messages/px4_rc_channels.h b/src/platforms/ros/px4_messages/px4_rc_channels.h new file mode 100644 index 000000000..a5a5ee202 --- /dev/null +++ b/src/platforms/ros/px4_messages/px4_rc_channels.h @@ -0,0 +1,25 @@ +#include "px4/rc_channels.h" +#include "platforms/px4_message.h" + +#pragma once +namespace px4 +{ + +class px4_rc_channels : + public PX4Message<rc_channels> +{ +public: + px4_rc_channels() : + PX4Message<rc_channels>() + {} + + px4_rc_channels(rc_channels msg) : + PX4Message<rc_channels>(msg) + {} + + ~px4_rc_channels() {} + + PX4TopicHandle handle() {return "rc_channels";} +}; + +} |