diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index 4a1e700755..634e5e5db6 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -78,18 +78,7 @@ public: * @param topic Name of the topic * @param fb Callback, executed on receiving a new message */ - // template - // Subscriber *subscribe(const char *topic, void(*fp)(const M &)) - // { - // SubscriberBase *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 (Subscriber *)sub; - // } template - // Subscriber *subscribe(void(*fp)(const typename std::remove_referencedata())>::type &)) Subscriber *subscribe(void(*fp)(const T &)) { SubscriberBase *sub = new SubscriberROS(std::bind(fp, std::placeholders::_1)); @@ -133,19 +122,9 @@ public: /** * Advertise topic - * @param topic Name of the topic */ - // template - // Publisher *advertise(const char *topic) - // { - // ros::Publisher ros_pub = ros::NodeHandle::advertise(topic, kQueueSizeDefault); - // Publisher *pub = new Publisher(ros_pub); - // _pubs.push_back(pub); - // return pub; - // } template Publisher* advertise() - // Publisher *advertise() { ros::Publisher ros_pub = ros::NodeHandle::advertisedata())>::type &>(T::handle(), kQueueSizeDefault); Publisher *pub = new Publisher(ros_pub); @@ -214,39 +193,14 @@ public: /** * 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 - // Subscriber *subscribe(const struct orb_metadata *meta, - // std::function callback, - // unsigned interval) - // { - // SubscriberUORBCallback *sub_px4 = new SubscriberUORBCallback(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 *)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 fp Callback, executed on receiving a new message * @param interval Minimal interval between calls to callback */ template - // Subscriber *subscribe(std::functiondata())>::type &)> callback, unsigned interval=10) //XXX interval - // Subscriber *subscribe(void(*fp)(const typename std::remove_referencedata())>::type &), unsigned interval=10) //XXX interval Subscriber *subscribe(void(*fp)(const T &), unsigned interval=10) //XXX interval { uORB::SubscriptionBase * uorb_sub = new uORB::SubscriptionBase(T::handle(), interval); - // SubscriberUORBCallback *sub_px4 = new SubscriberUORBCallback(uorb_sub, interval, callback); SubscriberUORBCallback *sub_px4 = new SubscriberUORBCallback(uorb_sub, interval, std::bind(fp, std::placeholders::_1)); /* Check if this is the smallest interval so far and update _sub_min_interval */ @@ -280,19 +234,6 @@ public: /** * Advertise topic - * @param meta Describes the topic which is advertised - */ - // template - // 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 Publisher *advertise()