Complementing ROS wrapper, not compiling yetros_wrapper

10 files changed, 163 insertions, 46 deletions

diff --git a/src/examples/publisher/publisher.cpp b/src/examples/publisher/publisher.cpp
index 91e063162..7e2b2f0d4 100644
--- a/src/examples/publisher/publisher.cpp
+++ b/src/examples/publisher/publisher.cpp
@@ -67,7 +67,7 @@ int main(int argc, char **argv)
 	 */
 	int count = 0;
 
-	while (px4::ok()) {
+	while (n.ok()) {
 		/**
 		 * This is a message object. You stuff it with data, and then publish it.
 		 */
@@ -84,7 +84,7 @@ int main(int argc, char **argv)
 		 */
 		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 bf16bf84e..626ceeb25 100644
--- a/src/examples/subscriber/subscriber.cpp
+++ b/src/examples/subscriber/subscriber.cpp
@@ -57,7 +57,7 @@ int main(int argc, char **argv)
 	 * The first NodeHandle constructed will fully initialize this node, and the last
 	 * NodeHandle destructed will close down the node.
 	 */
-	ros::NodeHandle n;
+	px4::NodeHandle n;
 
 	/**
 	 * The subscribe() call is how you tell ROS that you want to receive messages
@@ -74,14 +74,14 @@ int main(int argc, char **argv)
 	 * is the number of messages that will be buffered up before beginning to throw
 	 * away the oldest ones.
 	 */
-	ros::Subscriber sub = n.subscribe("rc_channels", 1000, rc_channels_callback);
+	px4::Subscriber sub = n.subscribe("rc_channels", 1000, rc_channels_callback);
 
 	/**
 	 * px4::spin() will enter a loop, pumping callbacks.  With this version, all
 	 * 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/nuttx/px4_nodehandle.cpp b/src/platforms/nuttx/px4_nodehandle.cpp
new file mode 100644
index 000000000..bdc659c40
--- /dev/null
+++ b/src/platforms/nuttx/px4_nodehandle.cpp
@@ -0,0 +1,58 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file px4_nodehandle.cpp
+ *
+ * PX4 Middleware Wrapper Node Handle
+ */
+
+namespace px4 {
+
+void NodeHandle::spinOnce()
+{
+	// XXX implement orb_check();
+}
+
+void NodeHandle::spin()
+{
+	// XXX polling wait
+}
+
+bool NodeHandle::ok()
+{
+	return !task_should_exit;
+}
+
+}
+
diff --git a/src/platforms/nuttx/px4_nuttx_impl.cpp b/src/platforms/nuttx/px4_nuttx_impl.cpp
index 3a6529716..c644dbced 100644
--- a/src/platforms/nuttx/px4_nuttx_impl.cpp
+++ b/src/platforms/nuttx/px4_nuttx_impl.cpp
@@ -39,8 +39,6 @@
 
 #include <px4.h>
 
-extern bool task_should_exit;
-
 namespace px4
 {
 
@@ -55,21 +53,4 @@ uint64_t get_time_micros()
 	return hrt_absolute_time();
 }
 
-bool ok()
-{
-	return !task_should_exit;
-}
-
-void spin_once()
-{
-	// XXX check linked list of topics with orb_check() here
-
-}
-
-void spin()
-{
-	// XXX block waiting for updated topics here
-
-}
-
 }
diff --git a/src/platforms/px4_middleware.h b/src/platforms/px4_middleware.h
index d1c0656af..d7bb1fbae 100644
--- a/src/platforms/px4_middleware.h
+++ b/src/platforms/px4_middleware.h
@@ -41,6 +41,8 @@
 
 #include <stdint.h>
 
+#include "px4_nodehandle.h"
+
 namespace px4
 {
 
@@ -48,12 +50,6 @@ void init(int argc, char *argv[], const char *process_name);
 
 uint64_t get_time_micros();
 
-bool ok();
-
-void spin_once();
-
-void spin();
-
 class Rate
 {
 
diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h
index d278828b7..50424c215 100644
--- a/src/platforms/px4_nodehandle.h
+++ b/src/platforms/px4_nodehandle.h
@@ -37,10 +37,32 @@
  * PX4 Middleware Wrapper Node Handle
  */
 
+#pragma once
+
+#include "px4_publisher.h"
+#include "px4_subscriber.h"
+
 namespace px4
 {
-class NodeHandle
+
+template<class T>
+	class NodeHandle
 {
 
+public:
+	void spin_once();
+
+	void spin();
+
+	template<class M > Publisher
+	advertise (const char* topic, uint32_t queue_size, bool latch=false);
+
+	template <class M> Subscriber
+	subscribe(const char* topic_name, uint32_t queue_size,
+		const std::function< void(const std::shared_ptr< M const > &)> &callback);
+
+protected:
+	bool task_should_exit;
 };
+
 }
diff --git a/src/platforms/px4_publisher.h b/src/platforms/px4_publisher.h
index 1b0952155..1a77fa3df 100644
--- a/src/platforms/px4_publisher.h
+++ b/src/platforms/px4_publisher.h
@@ -37,6 +37,8 @@
  * PX4 Middleware Wrapper Node Handle
  */
 
+#pragma once
+
 namespace px4
 {
 class Publisher
diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h
index 8759f8b05..30c5eee58 100644
--- a/src/platforms/px4_subscriber.h
+++ b/src/platforms/px4_subscriber.h
@@ -37,6 +37,8 @@
  * PX4 Middleware Wrapper Subscriber
  */
 
+#pragma once
+
 namespace px4
 {
 class Subscriber
diff --git a/src/platforms/ros/px4_nodehandle.cpp b/src/platforms/ros/px4_nodehandle.cpp
new file mode 100644
index 000000000..274a447c8
--- /dev/null
+++ b/src/platforms/ros/px4_nodehandle.cpp
@@ -0,0 +1,71 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file px4_nodehandle.cpp
+ *
+ * PX4 Middleware Wrapper Node Handle
+ */
+
+namespace px4 {
+
+void NodeHandle::spinOnce()
+{
+	ros::spinOnce();
+}
+
+void NodeHandle::spin()
+{
+	ros::spin();
+}
+
+bool NodeHandle::ok()
+{
+	return ros::ok();
+}
+
+template<class M > Publisher
+NodeHandle::advertise (const char* topic, uint32_t queue_size, bool latch=false)
+{
+	
+}
+
+template <class M> Subscriber
+NodeHandle::subscribe(const char* topic_name, uint32_t queue_size,
+		const std::function< void(const std::shared_ptr< M const > &)> &callback)
+{
+
+}
+
+}
+
diff --git a/src/platforms/ros/px4_ros_impl.cpp b/src/platforms/ros/px4_ros_impl.cpp
index eda17e5a9..854986a7f 100644
--- a/src/platforms/ros/px4_ros_impl.cpp
+++ b/src/platforms/ros/px4_ros_impl.cpp
@@ -53,19 +53,4 @@ uint64_t get_time_micros()
 	return time.sec * 1e6 + time.nsec / 1000;
 }
 
-bool ok()
-{
-	return ros::ok();
-}
-
-void spin_once()
-{
-	ros::spinOnce();
-}
-
-void spin()
-{
-	ros::spin();
-}
-
 }