diff --git a/src/examples/subscriber/subscriber.cpp b/src/examples/subscriber/subscriber.cpp index 39059daa30..5d1d14d7fa 100644 --- a/src/examples/subscriber/subscriber.cpp +++ b/src/examples/subscriber/subscriber.cpp @@ -75,6 +75,7 @@ int main(int argc, char **argv) * away the oldest ones. */ px4::Subscriber* sub = n.subscribe("rc_channels", rc_channels_callback); + PX4_INFO("subscribed"); /** * px4::spin() will enter a loop, pumping callbacks. With this version, all diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index df198615c5..809d0eb150 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -50,8 +50,9 @@ class NodeHandle : private ros::NodeHandle public: template Subscriber* subscribe(const char *topic, void(*fp)(M)) { - ros::NodeHandle::subscribe("rc_channels", 1000, fp); - return new Subscriber(); + ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, 1000, fp); + //XXX create list here, for ros and nuttx + return new Subscriber(ros_sub); } }; #else diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h index 8f883154ee..8933b2ab22 100644 --- a/src/platforms/px4_subscriber.h +++ b/src/platforms/px4_subscriber.h @@ -47,8 +47,11 @@ namespace px4 #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) class Subscriber { +private: + ros::Subscriber _ros_sub; public: - Subscriber() {}; + Subscriber(ros::Subscriber ros_sub) : _ros_sub(ros_sub) + {} ~Subscriber() {}; }; #else