From ee534b827a51a7356e565b665e280f765bd8d302 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 26 Nov 2014 13:18:28 +0100 Subject: move spin functions to nodehandle --- src/examples/publisher/publisher.cpp | 2 +- src/examples/subscriber/subscriber.cpp | 2 +- src/platforms/px4_middleware.h | 4 ---- src/platforms/px4_nodehandle.h | 5 +++++ src/platforms/ros/px4_ros_impl.cpp | 10 ---------- 5 files changed, 7 insertions(+), 16 deletions(-) (limited to 'src') diff --git a/src/examples/publisher/publisher.cpp b/src/examples/publisher/publisher.cpp index 5ee8b459b..521c7c838 100644 --- a/src/examples/publisher/publisher.cpp +++ b/src/examples/publisher/publisher.cpp @@ -90,7 +90,7 @@ PX4_MAIN_FUNCTION(publisher) */ rc_channels_pub->publish(msg); - px4::spin_once(); + n.spinOnce(); loop_rate.sleep(); ++count; diff --git a/src/examples/subscriber/subscriber.cpp b/src/examples/subscriber/subscriber.cpp index 093697d8e..0d0a81d7e 100644 --- a/src/examples/subscriber/subscriber.cpp +++ b/src/examples/subscriber/subscriber.cpp @@ -85,7 +85,7 @@ PX4_MAIN_FUNCTION(subscriber) * callbacks will be called from within this thread (the main one). px4::spin() * will exit when Ctrl-C is pressed, or the node is shutdown by the master. */ - px4::spin(); + n.spin(); return 0; } diff --git a/src/platforms/px4_middleware.h b/src/platforms/px4_middleware.h index a09d9ac58..8a83f1af4 100644 --- a/src/platforms/px4_middleware.h +++ b/src/platforms/px4_middleware.h @@ -51,10 +51,6 @@ __EXPORT uint64_t get_time_micros(); __EXPORT bool ok(); -__EXPORT void spin_once(); - -__EXPORT void spin(); - class Rate { diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index 972792d53..b665f3fe7 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -53,6 +53,7 @@ namespace px4 { +//XXX create abstract base class #if defined(__linux) || (defined(__APPLE__) && defined(__MACH__)) class NodeHandle : private ros::NodeHandle @@ -83,6 +84,10 @@ public: _pubs.push_back(pub); return pub; } + + void spin() { ros::spin(); } + + void spinOnce() { ros::spinOnce(); } private: static const uint32_t kQueueSizeDefault = 1000; std::list _subs; diff --git a/src/platforms/ros/px4_ros_impl.cpp b/src/platforms/ros/px4_ros_impl.cpp index eda17e5a9..3aa976138 100644 --- a/src/platforms/ros/px4_ros_impl.cpp +++ b/src/platforms/ros/px4_ros_impl.cpp @@ -58,14 +58,4 @@ bool ok() return ros::ok(); } -void spin_once() -{ - ros::spinOnce(); -} - -void spin() -{ - ros::spin(); -} - } -- cgit v1.2.3