aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--CMakeLists.txt85
-rw-r--r--makefiles/config_px4fmu-v2_multiplatform.mk3
-rw-r--r--makefiles/config_px4fmu-v2_test.mk2
-rw-r--r--msg/templates/msg.h.template74
-rw-r--r--src/examples/publisher/publisher_example.cpp19
-rw-r--r--src/examples/publisher/publisher_example.h1
-rw-r--r--src/examples/subscriber/subscriber_example.cpp14
-rw-r--r--src/examples/subscriber/subscriber_example.h3
-rw-r--r--src/platforms/nuttx/px4_messages/px4_rc_channels.h26
-rw-r--r--src/platforms/px4_includes.h4
-rw-r--r--src/platforms/px4_message.h77
-rw-r--r--src/platforms/px4_nodehandle.h180
-rw-r--r--src/platforms/px4_publisher.h41
-rw-r--r--src/platforms/px4_subscriber.h114
-rw-r--r--src/platforms/ros/px4_messages/px4_rc_channels.h25
15 files changed, 496 insertions, 172 deletions
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 2de236ff6..0ac9912df 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -116,6 +116,7 @@ catkin_package(
include_directories(
${catkin_INCLUDE_DIRS}
src/platforms
+ src/platforms/ros/px4_messages
src/include
src/modules
src/
@@ -157,52 +158,52 @@ target_link_libraries(subscriber
px4
)
-## MC Attitude Control
-add_executable(mc_att_control
- src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp
- src/modules/mc_att_control_multiplatform/mc_att_control.cpp
- src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp)
-add_dependencies(mc_att_control ${PROJECT_NAME}_generate_messages_cpp_cpp)
-target_link_libraries(mc_att_control
- ${catkin_LIBRARIES}
- px4
-)
+# ## MC Attitude Control
+# add_executable(mc_att_control
+ # src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp
+ # src/modules/mc_att_control_multiplatform/mc_att_control.cpp
+ # src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp)
+# add_dependencies(mc_att_control ${PROJECT_NAME}_generate_messages_cpp_cpp)
+# target_link_libraries(mc_att_control
+ # ${catkin_LIBRARIES}
+ # px4
+# )
-## Attitude Estimator dummy
-add_executable(attitude_estimator
- src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp)
-add_dependencies(attitude_estimator ${PROJECT_NAME}_generate_messages_cpp_cpp)
-target_link_libraries(attitude_estimator
- ${catkin_LIBRARIES}
- px4
-)
+# ## Attitude Estimator dummy
+# add_executable(attitude_estimator
+ # src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp)
+# add_dependencies(attitude_estimator ${PROJECT_NAME}_generate_messages_cpp_cpp)
+# target_link_libraries(attitude_estimator
+ # ${catkin_LIBRARIES}
+ # px4
+# )
-## Manual input
-add_executable(manual_input
- src/platforms/ros/nodes/manual_input/manual_input.cpp)
-add_dependencies(manual_input ${PROJECT_NAME}_generate_messages_cpp_cpp)
-target_link_libraries(manual_input
- ${catkin_LIBRARIES}
- px4
-)
+# ## Manual input
+# add_executable(manual_input
+ # src/platforms/ros/nodes/manual_input/manual_input.cpp)
+# add_dependencies(manual_input ${PROJECT_NAME}_generate_messages_cpp_cpp)
+# target_link_libraries(manual_input
+ # ${catkin_LIBRARIES}
+ # px4
+# )
-## Multicopter Mixer dummy
-add_executable(mc_mixer
- src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp)
-add_dependencies(mc_mixer ${PROJECT_NAME}_generate_messages_cpp_cpp)
-target_link_libraries(mc_mixer
- ${catkin_LIBRARIES}
- px4
-)
+# ## Multicopter Mixer dummy
+# add_executable(mc_mixer
+ # src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp)
+# add_dependencies(mc_mixer ${PROJECT_NAME}_generate_messages_cpp_cpp)
+# target_link_libraries(mc_mixer
+ # ${catkin_LIBRARIES}
+ # px4
+# )
-## Commander
-add_executable(commander
- src/platforms/ros/nodes/commander/commander.cpp)
-add_dependencies(manual_input ${PROJECT_NAME}_generate_messages_cpp_cpp)
-target_link_libraries(commander
- ${catkin_LIBRARIES}
- px4
-)
+# ## Commander
+# add_executable(commander
+ # src/platforms/ros/nodes/commander/commander.cpp)
+# add_dependencies(manual_input ${PROJECT_NAME}_generate_messages_cpp_cpp)
+# target_link_libraries(commander
+ # ${catkin_LIBRARIES}
+ # px4
+# )
#############
## Install ##
diff --git a/makefiles/config_px4fmu-v2_multiplatform.mk b/makefiles/config_px4fmu-v2_multiplatform.mk
index bed2fa506..bfa26f83c 100644
--- a/makefiles/config_px4fmu-v2_multiplatform.mk
+++ b/makefiles/config_px4fmu-v2_multiplatform.mk
@@ -86,7 +86,8 @@ MODULES += modules/position_estimator_inav
#MODULES += modules/fw_pos_control_l1
#MODULES += modules/fw_att_control
# MODULES += modules/mc_att_control
-MODULES += modules/mc_att_control_multiplatform
+# MODULES += modules/mc_att_control_multiplatform
+MODULES += examples/publisher
MODULES += modules/mc_pos_control
MODULES += modules/vtol_att_control
diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk
index fa2c83262..4defe49e7 100644
--- a/makefiles/config_px4fmu-v2_test.mk
+++ b/makefiles/config_px4fmu-v2_test.mk
@@ -56,7 +56,7 @@ MODULES += systemcmds/ver
# Example modules
#
MODULES += examples/matlab_csv_serial
-MODULES += examples/subscriber
+#MODULES += examples/subscriber
MODULES += examples/publisher
#
diff --git a/msg/templates/msg.h.template b/msg/templates/msg.h.template
index 19068a3a1..c27daed83 100644
--- a/msg/templates/msg.h.template
+++ b/msg/templates/msg.h.template
@@ -95,17 +95,17 @@ for field in spec.parsed_fields():
@##############################
@{
-type_map = {'int8': 'int8_t',
- 'int16': 'int16_t',
- 'int32': 'int32_t',
- 'int64': 'int64_t',
- 'uint8': 'uint8_t',
- 'uint16': 'uint16_t',
- 'uint32': 'uint32_t',
- 'uint64': 'uint64_t',
- 'float32': 'float',
- 'bool': 'bool',
- 'fence_vertex': 'fence_vertex'}
+type_map = {'int8': ['int8_t', '0'],
+ 'int16': ['int16_t', '0'],
+ 'int32': ['int32_t', '0'],
+ 'int64': ['int64_t', '0'],
+ 'uint8': ['uint8_t', '0'],
+ 'uint16': ['uint16_t', '0'],
+ 'uint32': ['uint32_t', '0'],
+ 'uint64': ['uint64_t', '0'],
+ 'float32': ['float', '0.0f'],
+ 'bool': ['bool', 'false'],
+ 'fence_vertex': ['fence_vertex', '']}
# Function to print a standard ros type
def print_field_def(field):
@@ -129,20 +129,70 @@ def print_field_def(field):
if type in type_map:
# need to add _t: int8 --> int8_t
- type_px4 = type_map[type]
+ type_px4 = type_map[type][0]
else:
raise Exception("Type {0} not supported, add to to template file!".format(type))
print('\t%s%s%s %s%s;'%(type_prefix, type_px4, type_appendix, field.name, array_size))
+# Function to init fields
+def get_field_init(field):
+ type = field.type
+ # detect embedded types
+ sl_pos = type.find('/')
+ type_appendix = ''
+ type_prefix = ''
+ if (sl_pos >= 0):
+ type = type[sl_pos + 1:]
+ type_prefix = 'struct '
+ type_appendix = '_s'
+
+ # detect arrays
+ a_pos = type.find('[')
+ array_size = ''
+ if (a_pos >= 0):
+ # field is array
+ array_size = type[a_pos:]
+ type = type[:a_pos]
+ return '\n\t%s{},'%(field.name)
+
+ if type in type_map:
+ # need to add _t: int8 --> int8_t
+ type_px4 = type_map[type][0]
+ init_value = type_map[type][1]
+ else:
+ raise Exception("Type {0} not supported, add to to template file!".format(type))
+
+ return '\n\t%s(%s),'%(field.name, init_value)
}
+@#ifdef __cplusplus
+@#class @(spec.short_name)_s {
+@#public:
+@#else
struct @(spec.short_name)_s {
+@#endif
@{
# loop over all fields and print the type and name
for field in spec.parsed_fields():
if (not field.is_header):
print_field_def(field)
}@
+
+@##ifdef __cplusplus
+@#@(spec.short_name)_s() :
+@#@{
+@#field_init = ''
+@## loop over all fields and init
+@#for field in spec.parsed_fields():
+@# if (not field.is_header):
+@# field_init += get_field_init(field)
+@#
+@#print(field_init[:-1])
+@#}@
+@#{}
+@#virtual ~@(spec.short_name)_s() {}
+@##endif
+
};
/**
diff --git a/src/examples/publisher/publisher_example.cpp b/src/examples/publisher/publisher_example.cpp
index 2e5779ebe..e85e42b38 100644
--- a/src/examples/publisher/publisher_example.cpp
+++ b/src/examples/publisher/publisher_example.cpp
@@ -45,7 +45,8 @@ using namespace px4;
PublisherExample::PublisherExample() :
_n(),
- _rc_channels_pub(PX4_ADVERTISE(_n, rc_channels))
+ _rc_channels_pub(_n.advertise<px4_rc_channels>())
+ // _rc_channels_pub(PX4_ADVERTISE(_n, rc_channels))
{
}
@@ -55,11 +56,19 @@ int PublisherExample::main()
px4::Rate loop_rate(10);
while (px4::ok()) {
- PX4_TOPIC_T(rc_channels) msg;
- msg.timestamp_last_valid = px4::get_time_micros();
- PX4_INFO("%llu", msg.timestamp_last_valid);
+ // PX4_TOPIC_T(rc_channels) msg;
+ // msg.timestamp_last_valid = px4::get_time_micros();
+ // PX4_INFO("%llu", msg.timestamp_last_valid);
+
+ // _rc_channels_pub->publish(msg);
+
+ //XXX
+ px4_rc_channels msg2;
+ msg2.data().timestamp_last_valid = px4::get_time_micros();
+ PX4_INFO("%llu", msg2.data().timestamp_last_valid);
+ // msg2.pub->publish2();
+ _rc_channels_pub->publish(msg2);
- _rc_channels_pub->publish(msg);
_n.spinOnce();
loop_rate.sleep();
diff --git a/src/examples/publisher/publisher_example.h b/src/examples/publisher/publisher_example.h
index 304ecef47..86586d0d3 100644
--- a/src/examples/publisher/publisher_example.h
+++ b/src/examples/publisher/publisher_example.h
@@ -48,5 +48,6 @@ public:
int main();
protected:
px4::NodeHandle _n;
+ // px4::Publisher<px4::px4_rc_channels> * _rc_channels_pub;
px4::Publisher * _rc_channels_pub;
};
diff --git a/src/examples/subscriber/subscriber_example.cpp b/src/examples/subscriber/subscriber_example.cpp
index 3c80561ca..1c8f4c62b 100644
--- a/src/examples/subscriber/subscriber_example.cpp
+++ b/src/examples/subscriber/subscriber_example.cpp
@@ -62,11 +62,13 @@ SubscriberExample::SubscriberExample() :
/* Do some subscriptions */
/* Function */
- PX4_SUBSCRIBE(_n, rc_channels, rc_channels_callback_function, _interval);
- /* Class Method */
- PX4_SUBSCRIBE(_n, rc_channels, SubscriberExample::rc_channels_callback, this, 1000);
- /* No callback */
- _sub_rc_chan = PX4_SUBSCRIBE(_n, rc_channels, 500);
+ // PX4_SUBSCRIBE(_n, rc_channels, rc_channels_callback_function, _interval);
+ _n.subscribe<px4_rc_channels>(rc_channels_callback_function);
+
+ // [> Class Method <]
+ // PX4_SUBSCRIBE(_n, rc_channels, SubscriberExample::rc_channels_callback, this, 1000);
+ // [> No callback <]
+ // _sub_rc_chan = PX4_SUBSCRIBE(_n, rc_channels, 500);
PX4_INFO("subscribed");
}
@@ -78,5 +80,5 @@ SubscriberExample::SubscriberExample() :
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,
- _sub_rc_chan->get().timestamp_last_valid);
+ _sub_rc_chan->get().data().timestamp_last_valid);
}
diff --git a/src/examples/subscriber/subscriber_example.h b/src/examples/subscriber/subscriber_example.h
index eb2c956e0..b52ae991b 100644
--- a/src/examples/subscriber/subscriber_example.h
+++ b/src/examples/subscriber/subscriber_example.h
@@ -56,7 +56,8 @@ protected:
int32_t _interval;
px4_param_t _p_test_float;
float _test_float;
- px4::PX4_SUBSCRIBER(rc_channels) * _sub_rc_chan;
+ // px4::PX4_SUBSCRIBER(rc_channels) * _sub_rc_chan;
+ px4::Subscriber<px4_rc_channels> * _sub_rc_chan;
void rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg);
diff --git a/src/platforms/nuttx/px4_messages/px4_rc_channels.h b/src/platforms/nuttx/px4_messages/px4_rc_channels.h
new file mode 100644
index 000000000..bfca48469
--- /dev/null
+++ b/src/platforms/nuttx/px4_messages/px4_rc_channels.h
@@ -0,0 +1,26 @@
+#include <uORB/uORB.h>
+#include <uORB/topics/rc_channels.h>
+#include "platforms/px4_message.h"
+
+#pragma once
+namespace px4
+{
+
+class px4_rc_channels :
+ public PX4Message<rc_channels_s>
+{
+public:
+ px4_rc_channels() :
+ PX4Message<rc_channels_s>()
+ {}
+
+ px4_rc_channels(rc_channels_s msg) :
+ PX4Message<rc_channels_s>(msg)
+ {}
+
+ ~px4_rc_channels() {}
+
+ PX4TopicHandle handle() {return (PX4TopicHandle)ORB_ID(rc_channels);}
+};
+
+}
diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h
index fc31162c8..bdb2b8de9 100644
--- a/src/platforms/px4_includes.h
+++ b/src/platforms/px4_includes.h
@@ -49,6 +49,7 @@
#ifdef __cplusplus
#include "ros/ros.h"
#include "px4/rc_channels.h"
+#include "px4_rc_channels.h" //XXX
#include "px4/vehicle_attitude.h"
#include <px4/vehicle_attitude_setpoint.h>
#include <px4/manual_control_setpoint.h>
@@ -71,6 +72,9 @@
#include <nuttx/config.h>
#include <uORB/uORB.h>
#include <uORB/topics/rc_channels.h>
+#ifdef __cplusplus
+#include <platforms/nuttx/px4_messages/px4_rc_channels.h> //XXX
+#endif
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/actuator_controls.h>
diff --git a/src/platforms/px4_message.h b/src/platforms/px4_message.h
new file mode 100644
index 000000000..05fcf1140
--- /dev/null
+++ b/src/platforms/px4_message.h
@@ -0,0 +1,77 @@
+/****************************************************************************
+ *
+ * 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_message.h
+ *
+ * Defines the message base types
+ */
+#pragma once
+
+#if defined(__PX4_ROS)
+typedef const char* PX4TopicHandle;
+#else
+#include <uORB/uORB.h>
+typedef const struct orb_metatdata* PX4TopicHandle;
+#endif
+
+namespace px4
+{
+
+template <typename M>
+class PX4Message
+{
+ // friend class NodeHandle;
+// #if defined(__PX4_ROS)
+ // template<typename T>
+ // friend class SubscriberROS;
+// #endif
+
+public:
+ PX4Message() :
+ _data()
+ {}
+
+ PX4Message(M msg) :
+ _data(msg)
+ {}
+
+ virtual ~PX4Message() {};
+
+ virtual M& data() {return _data;}
+ virtual PX4TopicHandle handle() = 0;
+private:
+ M _data;
+};
+
+}
diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h
index 7b14caed9..a40581239 100644
--- a/src/platforms/px4_nodehandle.h
+++ b/src/platforms/px4_nodehandle.h
@@ -48,6 +48,7 @@
#include "ros/ros.h"
#include <list>
#include <inttypes.h>
+#include <type_traits>
#else
/* includes when building for NuttX */
#include <poll.h>
@@ -77,15 +78,25 @@ public:
* @param topic Name of the topic
* @param fb Callback, executed on receiving a new message
*/
- template<typename M>
- Subscriber<M> *subscribe(const char *topic, void(*fp)(const M &))
+ // template<typename M>
+ // Subscriber<M> *subscribe(const char *topic, void(*fp)(const M &))
+ // {
+ // SubscriberBase *sub = new SubscriberROS<M>(std::bind(fp, std::placeholders::_1));
+ // ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS<M>::callback,
+ // (SubscriberROS<M> *)sub);
+ // ((SubscriberROS<M> *)sub)->set_ros_sub(ros_sub);
+ // _subs.push_back(sub);
+ // return (Subscriber<M> *)sub;
+ // }
+ template<typename T>
+ Subscriber<T> *subscribe(void(*fp)(const typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &))
{
- SubscriberBase *sub = new SubscriberROS<M>(std::bind(fp, std::placeholders::_1));
- ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS<M>::callback,
- (SubscriberROS<M> *)sub);
- ((SubscriberROS<M> *)sub)->set_ros_sub(ros_sub);
+ SubscriberBase *sub = new SubscriberROS<T>(std::bind(fp, std::placeholders::_1));
+ ros::Subscriber ros_sub = ros::NodeHandle::subscribe((new T())->handle(), kQueueSizeDefault,
+ &SubscriberROS<T>::callback, (SubscriberROS<T> *)sub);
+ ((SubscriberROS<T> *)sub)->set_ros_sub(ros_sub);
_subs.push_back(sub);
- return (Subscriber<M> *)sub;
+ return (Subscriber<T> *)sub;
}
/**
@@ -93,40 +104,49 @@ public:
* @param topic Name of the topic
* @param fb Callback, executed on receiving a new message
*/
- template<typename M, typename T>
- Subscriber<M> *subscribe(const char *topic, void(T::*fp)(const M &), T *obj)
- {
- SubscriberBase *sub = new SubscriberROS<M>(std::bind(fp, obj, std::placeholders::_1));
- ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS<M>::callback,
- (SubscriberROS<M> *)sub);
- ((SubscriberROS<M> *)sub)->set_ros_sub(ros_sub);
- _subs.push_back(sub);
- return (Subscriber<M> *)sub;
- }
+ // template<typename M, typename T>
+ // Subscriber<M> *subscribe(const char *topic, void(T::*fp)(const M &), T *obj)
+ // {
+ // SubscriberBase *sub = new SubscriberROS<M>(std::bind(fp, obj, std::placeholders::_1));
+ // ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS<M>::callback,
+ // (SubscriberROS<M> *)sub);
+ // ((SubscriberROS<M> *)sub)->set_ros_sub(ros_sub);
+ // _subs.push_back(sub);
+ // return (Subscriber<M> *)sub;
+ // }
/**
* Subscribe with no callback, just the latest value is stored on updates
* @param topic Name of the topic
*/
- template<typename M>
- Subscriber<M> *subscribe(const char *topic)
- {
- SubscriberBase *sub = new SubscriberROS<M>();
- ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS<M>::callback,
- (SubscriberROS<M> *)sub);
- ((SubscriberROS<M> *)sub)->set_ros_sub(ros_sub);
- _subs.push_back(sub);
- return (Subscriber<M> *)sub;
- }
+ // template<typename M>
+ // Subscriber<M> *subscribe(const char *topic)
+ // {
+ // SubscriberBase *sub = new SubscriberROS<M>();
+ // ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS<M>::callback,
+ // (SubscriberROS<M> *)sub);
+ // ((SubscriberROS<M> *)sub)->set_ros_sub(ros_sub);
+ // _subs.push_back(sub);
+ // return (Subscriber<M> *)sub;
+ // }
/**
* Advertise topic
* @param topic Name of the topic
*/
- template<typename M>
- Publisher *advertise(const char *topic)
+ // template<typename M>
+ // Publisher *advertise(const char *topic)
+ // {
+ // ros::Publisher ros_pub = ros::NodeHandle::advertise<M>(topic, kQueueSizeDefault);
+ // Publisher *pub = new Publisher(ros_pub);
+ // _pubs.push_back(pub);
+ // return pub;
+ // }
+ template<typename T>
+ Publisher* advertise()
+ // Publisher<T> *advertise()
{
- ros::Publisher ros_pub = ros::NodeHandle::advertise<M>(topic, kQueueSizeDefault);
+ ros::Publisher ros_pub = ros::NodeHandle::advertise<typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &>((new T())->handle(), kQueueSizeDefault);
Publisher *pub = new Publisher(ros_pub);
_pubs.push_back(pub);
return pub;
@@ -161,7 +181,7 @@ public:
~NodeHandle()
{
/* Empty subscriptions list */
- uORB::SubscriptionNode *sub = _subs.getHead();
+ SubscriberNode *sub = _subs.getHead();
int count = 0;
while (sub != nullptr) {
@@ -170,13 +190,13 @@ public:
break;
}
- uORB::SubscriptionNode *sib = sub->getSibling();
+ SubscriberNode *sib = sub->getSibling();
delete sub;
sub = sib;
}
/* Empty publications list */
- uORB::PublicationNode *pub = _pubs.getHead();
+ Publisher *pub = _pubs.getHead();
count = 0;
while (pub != nullptr) {
@@ -185,7 +205,7 @@ public:
break;
}
- uORB::PublicationNode *sib = pub->getSibling();
+ Publisher *sib = pub->getSibling();
delete pub;
pub = sib;
}
@@ -198,19 +218,41 @@ public:
* @param interval Minimal interval between calls to callback
*/
- template<typename M>
- Subscriber<M> *subscribe(const struct orb_metadata *meta,
- std::function<void(const M &)> callback,
- unsigned interval)
+ // template<typename M>
+ // Subscriber<M> *subscribe(const struct orb_metadata *meta,
+ // std::function<void(const M &)> callback,
+ // unsigned interval)
+ // {
+ // SubscriberUORBCallback<M> *sub_px4 = new SubscriberUORBCallback<M>(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<M> *)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 interval Minimal interval between calls to callback
+ */
+
+ template<typename T>
+ Subscriber<T> *subscribe(std::function<void(const typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &)> callback, unsigned interval=10) //XXX interval
{
- SubscriberUORBCallback<M> *sub_px4 = new SubscriberUORBCallback<M>(meta, interval, callback, &_subs);
+ const struct orb_metadata * meta = NULL;
+ uORB::SubscriptionBase * uorb_sub = new uORB::SubscriptionBase(meta, interval);
+ SubscriberUORBCallback<T> *sub_px4 = new SubscriberUORBCallback<T>(uorb_sub, callback);
/* 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()) {
+ if (_sub_min_interval == nullptr || _sub_min_interval->get_interval() > interval) {
_sub_min_interval = sub_px4;
}
+ _subs.add((SubscriberNode *)sub_px4);
- return (Subscriber<M> *)sub_px4;
+ return (Subscriber<T> *)sub_px4;
}
/**
@@ -219,29 +261,47 @@ public:
* @param interval Minimal interval between data fetches from orb
*/
- template<typename M>
- Subscriber<M> *subscribe(const struct orb_metadata *meta,
- unsigned interval)
- {
- SubscriberUORB<M> *sub_px4 = new SubscriberUORB<M>(meta, interval, &_subs);
+ // template<typename M>
+ // Subscriber<M> *subscribe(const struct orb_metadata *meta,
+ // unsigned interval)
+ // {
+ // SubscriberUORB<M> *sub_px4 = new SubscriberUORB<M>(meta, interval, &_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;
- }
+ // [> 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<M> *)sub_px4;
- }
+ // return (Subscriber<M> *)sub_px4;
+ // }
/**
* Advertise topic
* @param meta Describes the topic which is advertised
*/
- template<typename M>
- Publisher *advertise(const struct orb_metadata *meta)
+ // template<typename M>
+ // 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<typename T>
+ Publisher *advertise()
{
//XXX
- Publisher *pub = new Publisher(meta, &_pubs);
+ // uORB::PublicationBase * uorb_pub = new uORB::PublicationBase((new T())->handle());
+ const struct orb_metadata * meta = NULL;
+ uORB::PublicationBase * uorb_pub = new uORB::PublicationBase(meta);
+ Publisher *pub = new Publisher(uorb_pub);
+
+ _pubs.add(pub);
+
return pub;
}
@@ -251,7 +311,7 @@ public:
void spinOnce()
{
/* Loop through subscriptions, call callback for updated subscriptions */
- uORB::SubscriptionNode *sub = _subs.getHead();
+ SubscriberNode *sub = _subs.getHead();
int count = 0;
while (sub != nullptr) {
@@ -281,7 +341,7 @@ public:
/* Poll fd with smallest interval */
struct pollfd pfd;
- pfd.fd = _sub_min_interval->getHandle();
+ pfd.fd = _sub_min_interval->getUORBHandle();
pfd.events = POLLIN;
poll(&pfd, 1, timeout_ms);
spinOnce();
@@ -290,9 +350,9 @@ public:
private:
static const uint16_t kMaxSubscriptions = 100;
static const uint16_t kMaxPublications = 100;
- List<uORB::SubscriptionNode *> _subs; /**< Subcriptions of node */
- List<uORB::PublicationNode *> _pubs; /**< Publications of node */
- uORB::SubscriptionNode *_sub_min_interval; /**< Points to the sub wtih the smallest interval
+ List<SubscriberNode *> _subs; /**< Subcriptions of node */
+ List<Publisher *> _pubs; /**< Publications of node */
+ SubscriberNode *_sub_min_interval; /**< Points to the sub wtih the smallest interval
of all Subscriptions in _subs*/
};
#endif
diff --git a/src/platforms/px4_publisher.h b/src/platforms/px4_publisher.h
index c6f3d6108..aff045d23 100644
--- a/src/platforms/px4_publisher.h
+++ b/src/platforms/px4_publisher.h
@@ -46,6 +46,8 @@
#include <containers/List.hpp>
#endif
+#include <platforms/px4_message.h>
+
namespace px4
{
@@ -58,11 +60,19 @@ public:
PublisherBase() {};
~PublisherBase() {};
+ /** Publishes msg
+ * @param msg the message which is published to the topic
+ */
+ // virtual int publish2(const PX4Message * const msg) = 0;
+
};
#if defined(__PX4_ROS)
+// template <typename T>
class Publisher :
public PublisherBase
+ // public PublisherBase,
+ // public T
{
public:
/**
@@ -71,6 +81,7 @@ public:
*/
Publisher(ros::Publisher ros_pub) :
PublisherBase(),
+ // T(),
_ros_pub(ros_pub)
{}
@@ -79,15 +90,20 @@ public:
/** Publishes msg
* @param msg the message which is published to the topic
*/
- template<typename M>
- int publish(const M &msg) { _ros_pub.publish(msg); return 0; }
+ // int publish(const M &msg) { _ros_pub.publish(msg); return 0; }
+ template <typename T>
+ int publish(T &msg) {
+ _ros_pub.publish(msg.data());
+ return 0;}
private:
ros::Publisher _ros_pub; /**< Handle to the ros publisher */
};
#else
class Publisher :
- public uORB::PublicationNode,
- public PublisherBase
+ // public uORB::PublicationNode,
+ public PublisherBase,
+ public ListNode<Publisher *>
+
{
public:
/**
@@ -95,10 +111,14 @@ public:
* @param meta orb metadata for the topic which is used
* @param list publisher is added to this list
*/
- Publisher(const struct orb_metadata *meta,
- List<uORB::PublicationNode *> *list) :
- uORB::PublicationNode(meta, list),
- PublisherBase()
+ // Publisher(const struct orb_metadata *meta,
+ // List<uORB::PublicationNode *> *list) :
+ // uORB::PublicationNode(meta, list),
+ // PublisherBase()
+ // {}
+ Publisher(uORB::PublicationBase * uorb_pub) :
+ PublisherBase(),
+ _uorb_pub(uorb_pub)
{}
~Publisher() {};
@@ -109,7 +129,7 @@ public:
template<typename M>
int publish(const M &msg)
{
- uORB::PublicationBase::update((void *)&msg);
+ _uorb_pub->update((void *)&msg);
return 0;
}
@@ -117,6 +137,9 @@ public:
* Empty callback for list traversal
*/
void update() {} ;
+private:
+ uORB::PublicationBase * _uorb_pub; /**< Handle to the publisher */
+
};
#endif
}
diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h
index aaacf9e48..ef03922ad 100644
--- a/src/platforms/px4_subscriber.h
+++ b/src/platforms/px4_subscriber.h
@@ -39,6 +39,7 @@
#pragma once
#include <functional>
+#include <type_traits>
#if defined(__PX4_ROS)
/* includes when building for ros */
@@ -67,7 +68,7 @@ public:
/**
* Subscriber class which is used by nodehandle
*/
-template<typename M>
+template<typename T>
class Subscriber :
public SubscriberBase
{
@@ -81,7 +82,7 @@ public:
/**
* Get the last message value
*/
- virtual M get() = 0;
+ virtual T get() = 0;
/**
* Get void pointer to last message value
@@ -93,9 +94,9 @@ public:
/**
* Subscriber class that is templated with the ros n message type
*/
-template<typename M>
+template<typename T>
class SubscriberROS :
- public Subscriber<M>
+ public Subscriber<T>
{
friend class NodeHandle;
@@ -103,8 +104,8 @@ public:
/**
* Construct Subscriber by providing a callback function
*/
- SubscriberROS(std::function<void(const M &)> cbf) :
- Subscriber<M>(),
+ SubscriberROS(std::function<void(const typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &)> cbf) :
+ Subscriber<T>(),
_ros_sub(),
_cbf(cbf),
_msg_current()
@@ -114,7 +115,7 @@ public:
* Construct Subscriber without a callback function
*/
SubscriberROS() :
- Subscriber<M>(),
+ Subscriber<T>(),
_ros_sub(),
_cbf(NULL),
_msg_current()
@@ -127,7 +128,7 @@ public:
/**
* Get the last message value
*/
- M get() { return _msg_current; }
+ T get() { return _msg_current; }
/**
* Get void pointer to last message value
*/
@@ -137,10 +138,10 @@ protected:
/**
* Called on topic update, saves the current message and then calls the provided callback function
*/
- void callback(const M &msg)
+ void callback(const typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &msg)
{
/* Store data */
- _msg_current = msg;
+ _msg_current = (T)msg;
/* Call callback */
if (_cbf != NULL) {
@@ -158,20 +159,47 @@ protected:
}
ros::Subscriber _ros_sub; /**< Handle to ros subscriber */
- std::function<void(const M &)> _cbf; /**< Callback that the user provided on the subscription */
- M _msg_current; /**< Current Message value */
+ std::function<void(const typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &)> _cbf; /**< Callback that the user provided on the subscription */
+ T _msg_current; /**< Current Message value */
};
#else // Building for NuttX
+
+/**
+ * Because we maintain a list of subscribers we need a node class
+ */
+class SubscriberNode :
+ public ListNode<SubscriberNode *>
+{
+public:
+ SubscriberNode(unsigned interval) :
+ ListNode(),
+ _interval(interval)
+ {}
+
+ virtual ~SubscriberNode() {}
+
+ virtual void update() = 0;
+
+ virtual int getUORBHandle() = 0;
+
+ unsigned get_interval() { return _interval; }
+
+protected:
+ unsigned _interval;
+
+};
+
+
/**
* Subscriber class that is templated with the uorb subscription message type
*/
-template<typename M>
+template<typename T>
class SubscriberUORB :
- public Subscriber<M>,
- public uORB::Subscription<M>
+ public Subscriber<T>,
+ public SubscriberNode
{
public:
@@ -181,11 +209,15 @@ public:
* @param interval Minimal interval between calls to callback
* @param list subscriber is added to this list
*/
- SubscriberUORB(const struct orb_metadata *meta,
- unsigned interval,
- List<uORB::SubscriptionNode *> *list) :
- Subscriber<M>(),
- uORB::Subscription<M>(meta, interval, list)
+ // SubscriberUORB(const struct orb_metadata *meta,
+ // unsigned interval,
+ // List<uORB::SubscriptionNode *> *list) :
+ // Subscriber<M>(),
+ // uORB::Subscription<M>(meta, interval, list)
+ // {}
+ SubscriberUORB(uORB::SubscriptionBase * uorb_sub, unsigned interval) :
+ SubscriberNode(interval),
+ _uorb_sub(uorb_sub)
{}
~SubscriberUORB() {};
@@ -196,29 +228,36 @@ public:
*/
virtual void update()
{
- if (!uORB::Subscription<M>::updated()) {
+ if (!_uorb_sub->updated()) {
/* Topic not updated */
return;
}
/* get latest data */
- uORB::Subscription<M>::update();
+ _uorb_sub->update(get_void_ptr());
};
/* Accessors*/
/**
* Get the last message value
*/
- M get() { return uORB::Subscription<M>::getData(); }
+ T get() { return (T)(typename std::remove_reference<decltype(((T*)nullptr)->data())>::type)*_uorb_sub; }
/**
* Get void pointer to last message value
*/
- void *get_void_ptr() { return uORB::Subscription<M>::getDataVoidPtr(); }
+ void *get_void_ptr() { return (void *)(typename std::remove_reference<decltype(((T*)nullptr)->data())>::type*)_uorb_sub; }
+
+ int getUORBHandle() { return _uorb_sub->getHandle(); }
+
+protected:
+ uORB::SubscriptionBase * _uorb_sub; /**< Handle to the subscription */
+ typename std::remove_reference<decltype(((T*)nullptr)->data())>::type getUORBData() { return (typename std::remove_reference<decltype(((T*)nullptr)->data())>::type)*_uorb_sub; }
};
-template<typename M>
+//XXX reduce to one class with overloaded constructor?
+template<typename T>
class SubscriberUORBCallback :
- public SubscriberUORB<M>
+ public SubscriberUORB<T>
{
public:
/**
@@ -228,11 +267,16 @@ public:
* @param interval Minimal interval between calls to callback
* @param list subscriber is added to this list
*/
- SubscriberUORBCallback(const struct orb_metadata *meta,
- unsigned interval,
- std::function<void(const M &)> callback,
- List<uORB::SubscriptionNode *> *list) :
- SubscriberUORB<M>(meta, interval, list),
+ // SubscriberUORBCallback(const struct orb_metadata *meta,
+ // unsigned interval,
+ // std::function<void(const M &)> callback,
+ // List<uORB::SubscriptionNode *> *list) :
+ // SubscriberUORB<M>(meta, interval, list),
+ // _callback(callback)
+ // {}
+ SubscriberUORBCallback(uORB::SubscriptionBase * uorb_sub,
+ std::function<void(typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &)> callback) :
+ SubscriberUORB<T>(uorb_sub),
_callback(callback)
{}
@@ -245,13 +289,13 @@ public:
*/
virtual void update()
{
- if (!uORB::Subscription<M>::updated()) {
+ if (!SubscriberUORB<T>::_uorb_sub->updated()) {
/* Topic not updated, do not call callback */
return;
}
/* get latest data */
- uORB::Subscription<M>::update();
+ SubscriberUORB<T>::_uorb_sub->update();
/* Check if there is a callback */
@@ -260,12 +304,12 @@ public:
}
/* Call callback which performs actions based on this data */
- _callback(uORB::Subscription<M>::getData());
+ _callback(SubscriberUORB<T>::getUORBData());
};
protected:
- std::function<void(const M &)> _callback; /**< Callback handle,
+ std::function<void(const typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &)> _callback; /**< Callback handle,
called when new data is available */
};
#endif
diff --git a/src/platforms/ros/px4_messages/px4_rc_channels.h b/src/platforms/ros/px4_messages/px4_rc_channels.h
new file mode 100644
index 000000000..a5a5ee202
--- /dev/null
+++ b/src/platforms/ros/px4_messages/px4_rc_channels.h
@@ -0,0 +1,25 @@
+#include "px4/rc_channels.h"
+#include "platforms/px4_message.h"
+
+#pragma once
+namespace px4
+{
+
+class px4_rc_channels :
+ public PX4Message<rc_channels>
+{
+public:
+ px4_rc_channels() :
+ PX4Message<rc_channels>()
+ {}
+
+ px4_rc_channels(rc_channels msg) :
+ PX4Message<rc_channels>(msg)
+ {}
+
+ ~px4_rc_channels() {}
+
+ PX4TopicHandle handle() {return "rc_channels";}
+};
+
+}
generated by cgit v1.2.3 (git 2.39.1) at 2024-08-27 18:21:18 +0000