diff --git a/src/examples/subscriber/subscriber_example.cpp b/src/examples/subscriber/subscriber_example.cpp index 217e4d48fe..215336c174 100644 --- a/src/examples/subscriber/subscriber_example.cpp +++ b/src/examples/subscriber/subscriber_example.cpp @@ -62,13 +62,13 @@ SubscriberExample::SubscriberExample() : /* Do some subscriptions */ /* Function */ - _n.subscribe(rc_channels_callback_function); //ROS version + _n.subscribe(rc_channels_callback_function, _interval); /* No callback */ - _sub_rc_chan = _n.subscribe(); + _sub_rc_chan = _n.subscribe(500); /* Class Method */ - _n.subscribe(&SubscriberExample::rc_channels_callback, this); + _n.subscribe(&SubscriberExample::rc_channels_callback, this, 1000); PX4_INFO("subscribed"); } diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index 2f92ead5a1..80be9ec527 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -79,7 +79,7 @@ public: * @param fb Callback, executed on receiving a new message */ template - Subscriber *subscribe(void(*fp)(const T &)) + Subscriber *subscribe(void(*fp)(const T &), unsigned interval) { SubscriberBase *sub = new SubscriberROS((ros::NodeHandle*)this, std::bind(fp, std::placeholders::_1)); _subs.push_back(sub); @@ -92,7 +92,7 @@ public: * @param obj pointer class instance */ template - Subscriber *subscribe(void(C::*fp)(const T &), C *obj) + Subscriber *subscribe(void(C::*fp)(const T &), C *obj, unsigned interval) { SubscriberBase *sub = new SubscriberROS((ros::NodeHandle*)this, std::bind(fp, obj, std::placeholders::_1)); _subs.push_back(sub); @@ -103,7 +103,7 @@ public: * Subscribe with no callback, just the latest value is stored on updates */ template - Subscriber *subscribe() + Subscriber *subscribe(unsigned interval) { SubscriberBase *sub = new SubscriberROS((ros::NodeHandle*)this); _subs.push_back(sub); @@ -186,8 +186,9 @@ public: */ template - Subscriber *subscribe(void(*fp)(const T &), unsigned interval=10) //XXX interval + Subscriber *subscribe(void(*fp)(const T &), unsigned interval) { + (void)interval; SubscriberUORBCallback *sub_px4 = new SubscriberUORBCallback(interval, std::bind(fp, std::placeholders::_1)); update_sub_min_interval(interval, sub_px4); _subs.add((SubscriberNode *)sub_px4); @@ -200,8 +201,9 @@ public: * @param obj pointer class instance */ template - Subscriber *subscribe(void(C::*fp)(const T &), C *obj, unsigned interval=10) + Subscriber *subscribe(void(C::*fp)(const T &), C *obj, unsigned interval) { + (void)interval; SubscriberUORBCallback *sub_px4 = new SubscriberUORBCallback(interval, std::bind(fp, obj, std::placeholders::_1)); update_sub_min_interval(interval, sub_px4); _subs.add((SubscriberNode *)sub_px4); @@ -214,8 +216,9 @@ public: */ template - Subscriber *subscribe(unsigned interval=10) //XXX interval + Subscriber *subscribe(unsigned interval) { + (void)interval; SubscriberUORB *sub_px4 = new SubscriberUORB(interval); update_sub_min_interval(interval, sub_px4); _subs.add((SubscriberNode *)sub_px4);