WIP, make class based and extended subscriber/publisher example compile for ros

CMakeLists.txt

@@ -111,7 +111,9 @@ target_link_libraries(px4
 )
 
 ## Declare a test publisher
-add_executable(publisher src/examples/publisher/publisher.cpp)
+add_executable(publisher
+	src/examples/publisher/publisher_main.cpp
+	src/examples/publisher/publisher_example.cpp)
 
 ## Add cmake target dependencies of the executable/library
 ## as an example, message headers may need to be generated before nodes
@@ -124,7 +126,9 @@ target_link_libraries(publisher
 )
 
 ## Declare a test subscriber
-add_executable(subscriber src/examples/subscriber/subscriber.cpp)
+add_executable(subscriber
+	src/examples/subscriber/subscriber_main.cpp
+	src/examples/subscriber/subscriber_example.cpp)
 
 ## Add cmake target dependencies of the executable/library
 ## as an example, message headers may need to be generated before nodes
@@ -136,14 +140,14 @@ target_link_libraries(subscriber
   px4
 )
 
-add_executable(mc_att_control
-	src/modules/mc_att_control/mc_att_control_main.cpp
-	src/moudles/mc_att_control/mc_att_control_base.cpp)
-add_dependencies(mc_att_control px4_generate_messages_cpp)
-target_link_libraries(mc_att_control
-  ${catkin_LIBRARIES}
-  px4
-)
+# add_executable(mc_att_control
+	# src/modules/mc_att_control/mc_att_control_main.cpp
+	# src/moudles/mc_att_control/mc_att_control_base.cpp)
+# add_dependencies(mc_att_control px4_generate_messages_cpp)
+# target_link_libraries(mc_att_control
+  # ${catkin_LIBRARIES}
+  # px4
+# )
 
 
 #############

src/examples/publisher/publisher_example.cpp

@@ -41,6 +41,8 @@
 
 #include "publisher_example.h"
 
+using namespace px4;
+
 PublisherExample::PublisherExample() :
 	_n(),
 	_rc_channels_pub(PX4_ADVERTISE(_n, rc_channels))

src/examples/publisher/publisher_example.h

@@ -38,6 +38,7 @@
  * @author Thomas Gubler <thomasgubler@gmail.com>
  */
 #include <px4.h>
+
 class PublisherExample {
 public:
 	PublisherExample();

src/examples/publisher/publisher_main.cpp

@@ -51,6 +51,7 @@ using namespace px4;
 
 PX4_MAIN_FUNCTION(publisher);
 
+#if !defined(__linux) && !(defined(__APPLE__) && defined(__MACH__))
 extern "C" __EXPORT int publisher_main(int argc, char *argv[])
 {
 	px4::init(argc, argv, "publisher");
@@ -98,15 +99,16 @@ extern "C" __EXPORT int publisher_main(int argc, char *argv[])
 	warnx("unrecognized command");
 	return 1;
 }
+#endif
 
 PX4_MAIN_FUNCTION(publisher)
 {
-	warnx("starting");
+	PX4_INFO("starting");
 	PublisherExample p;
 	thread_running = true;
 	p.main();
 
-	warnx("exiting.");
+	PX4_INFO("exiting.");
 	thread_running = false;
 	return 0;
 }

src/examples/subscriber/subscriber_example.cpp

@@ -76,7 +76,8 @@ SubscriberExample::SubscriberExample() :
  * Also the current value of the _sub_rc_chan subscription is printed
  */
 void SubscriberExample::rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg) {
-	PX4_INFO("Subscriber callback: [%llu], value of _sub_rc_chan: [%llu]",
-			msg.timestamp_last_valid,
-			((SubscriberPX4<PX4_TOPIC_T(rc_channels)> *)_sub_rc_chan)->timestamp_last_valid);
+	//XXX
+	// PX4_INFO("Subscriber callback: [%llu], value of _sub_rc_chan: [%llu]",
+			// msg.timestamp_last_valid,
+			// ((SubscriberPX4<PX4_TOPIC_T(rc_channels)> *)_sub_rc_chan)->timestamp_last_valid);
 }

src/examples/subscriber/subscriber_example.h

@@ -38,6 +38,9 @@
  * @author Thomas Gubler <thomasgubler@gmail.com>
  */
 #include <px4.h>
+
+using namespace px4;
+
 class SubscriberExample {
 public:
 	SubscriberExample();

src/examples/subscriber/subscriber_main.cpp

@@ -51,6 +51,7 @@ using namespace px4;
 
 PX4_MAIN_FUNCTION(subscriber);
 
+#if !defined(__linux) && !(defined(__APPLE__) && defined(__MACH__))
 extern "C" __EXPORT int subscriber_main(int argc, char *argv[])
 {
 	if (argc < 1) {
@@ -96,17 +97,18 @@ extern "C" __EXPORT int subscriber_main(int argc, char *argv[])
 	warnx("unrecognized command");
 	return 1;
 }
+#endif
 
 PX4_MAIN_FUNCTION(subscriber)
 {
 	px4::init(argc, argv, "subscriber");
 
-	warnx("starting");
+	PX4_INFO("starting");
 	SubscriberExample s;
 	thread_running = true;
 	s.spin();
 
-	warnx("exiting.");
+	PX4_INFO("exiting.");
 	thread_running = false;
 	return 0;
 }

src/platforms/px4_defines.h

@@ -66,9 +66,11 @@
 #define PX4_TOPIC_T(_name) _name
 
 /* Subscribe and providing a class method as callback (do not use directly, use PX4_SUBSCRIBE instead) */
-#define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _obj, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), &_cbf, &_obj);
+#define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _objptr, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), &_cbf, _objptr);
 /* Subscribe and providing a function as callback (do not use directly, use PX4_SUBSCRIBE instead) */
 #define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), _cbf);
+/* Subscribe without a callback (do not use directly, use PX4_SUBSCRIBE instead) */
+#define PX4_SUBSCRIBE_NOCB(_nodehandle, _name, _interval) _nodehandle.subscribe<const PX4_TOPIC_T(_name)&>(PX4_TOPIC(_name));
 
 /* Parameter handle datatype */
 typedef const char *px4_param_t;
@@ -113,9 +115,10 @@ static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, float value)
 #define PX4_TOPIC_T(_name) _name##_s
 
 /* Subscribe and providing a class method as callback (do not use directly, use PX4_SUBSCRIBE instead) */
-#define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _obj, _interval) _nodehandle.subscribe<PX4_TOPIC_T(_name)>(PX4_TOPIC(_name), std::bind(&_cbf, _obj, std::placeholders::_1), _interval)
+#define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _objptr, _interval) _nodehandle.subscribe<PX4_TOPIC_T(_name)>(PX4_TOPIC(_name), std::bind(&_cbf, _objptr, std::placeholders::_1), _interval)
 /* Subscribe and providing a function as callback (do not use directly, use PX4_SUBSCRIBE instead) */
 #define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe<PX4_TOPIC_T(_name)>(PX4_TOPIC(_name), std::bind(&_cbf, std::placeholders::_1), _interval)
+/* Subscribe without a callback (do not use directly, use PX4_SUBSCRIBE instead) */
 #define PX4_SUBSCRIBE_NOCB(_nodehandle, _name, _interval) _nodehandle.subscribe<PX4_TOPIC_T(_name)>(PX4_TOPIC(_name), nullptr, _interval)
 
 /* Parameter handle datatype */

src/platforms/px4_nodehandle.h

@@ -99,6 +99,19 @@ public:
 		return sub;
 	}
 
+	/**
+	 * Subscribe with no callback, just the latest value is stored on updates
+	 * @param topic		Name of the topic
+	 */
+	template<typename M>
+	Subscriber *subscribe(const char *topic)
+	{
+		//XXX missing implementation
+		// Subscriber *sub = new Subscriber(ros_sub);
+		// _subs.push_back(sub);
+		return (Subscriber *)NULL;
+	}
+
 	/**
 	 * Advertise topic
 	 * @param topic		Name of the topic