diff options
| -rw-r--r-- | src/examples/subscriber/subscriber_example.cpp | 20 | ||||
| -rw-r--r-- | src/examples/subscriber/subscriber_example.h | 2 | ||||
| -rw-r--r-- | src/platforms/px4_nodehandle.h | 43 | ||||
| -rw-r--r-- | src/platforms/px4_subscriber.h | 3 |
4 files changed, 34 insertions, 34 deletions
diff --git a/src/examples/subscriber/subscriber_example.cpp b/src/examples/subscriber/subscriber_example.cpp index 18f1a9eb9..217e4d48f 100644 --- a/src/examples/subscriber/subscriber_example.cpp +++ b/src/examples/subscriber/subscriber_example.cpp @@ -64,10 +64,11 @@ SubscriberExample::SubscriberExample() : /* Function */ _n.subscribe<px4_rc_channels>(rc_channels_callback_function); //ROS version - // [> Class Method <] - // PX4_SUBSCRIBE(_n, rc_channels, SubscriberExample::rc_channels_callback, this, 1000); - // [> No callback <] - // _sub_rc_chan = PX4_SUBSCRIBE(_n, rc_channels, 500); + /* No callback */ + _sub_rc_chan = _n.subscribe<px4_rc_channels>(); + + /* Class Method */ + _n.subscribe<px4_rc_channels>(&SubscriberExample::rc_channels_callback, this); PX4_INFO("subscribed"); } @@ -76,8 +77,9 @@ SubscriberExample::SubscriberExample() : * This tutorial demonstrates simple receipt of messages over the PX4 middleware system. * Also the current value of the _sub_rc_chan subscription is printed */ -// 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().data().timestamp_last_valid); -// } +void SubscriberExample::rc_channels_callback(const px4_rc_channels &msg) { + PX4_INFO("Callback (method): [%llu]", + msg.data().timestamp_last_valid); + PX4_INFO("Callback (method): value of _sub_rc_chan: [%llu]", + _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 883d83be7..8da3df438 100644 --- a/src/examples/subscriber/subscriber_example.h +++ b/src/examples/subscriber/subscriber_example.h @@ -58,7 +58,7 @@ protected: float _test_float; px4::Subscriber<px4_rc_channels> * _sub_rc_chan; - // void rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg); + void rc_channels_callback(const px4_rc_channels &msg); }; diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index a25d57845..e619623b9 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -91,34 +91,33 @@ public: /** * Subscribe with callback to class method - * @param topic Name of the topic * @param fb Callback, executed on receiving a new message + * @param obj pointer class instance */ - // 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 T, typename C> + Subscriber<T> *subscribe(void(C::*fp)(const T &), C *obj) + { + SubscriberBase *sub = new SubscriberROS<T>(std::bind(fp, obj, std::placeholders::_1)); + ros::Subscriber ros_sub = ros::NodeHandle::subscribe(T::handle(), kQueueSizeDefault, + &SubscriberROS<T>::callback, (SubscriberROS<T> *)sub); + ((SubscriberROS<T> *)sub)->set_ros_sub(ros_sub); + _subs.push_back(sub); + return (Subscriber<T> *)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 T> + Subscriber<T> *subscribe() + { + SubscriberBase *sub = new SubscriberROS<T>(); + ros::Subscriber ros_sub = ros::NodeHandle::subscribe(T::handle(), kQueueSizeDefault, + &SubscriberROS<T>::callback, (SubscriberROS<T> *)sub); + ((SubscriberROS<T> *)sub)->set_ros_sub(ros_sub); + _subs.push_back(sub); + return (Subscriber<T> *)sub; + } /** * Advertise topic diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h index d03b3edee..c499712a9 100644 --- a/src/platforms/px4_subscriber.h +++ b/src/platforms/px4_subscriber.h @@ -130,11 +130,10 @@ protected: void callback(const typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &msg) { /* Store data */ - this->_msg_current = (T)msg; + this->_msg_current.data() = msg; /* Call callback */ if (_cbf != NULL) { - // _cbf(_msg_current); _cbf(this->get()); } |