From 7c3223b8609ee418b520d19cae7e52d2a7a85e99 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 18 Jan 2015 18:43:45 +0100 Subject: added a messageplayer prototype for ros --- CMakeLists.txt | 85 +++++----- makefiles/config_px4fmu-v2_multiplatform.mk | 3 +- makefiles/config_px4fmu-v2_test.mk | 2 +- msg/templates/msg.h.template | 74 +++++++-- src/examples/publisher/publisher_example.cpp | 19 ++- src/examples/publisher/publisher_example.h | 1 + src/examples/subscriber/subscriber_example.cpp | 14 +- src/examples/subscriber/subscriber_example.h | 3 +- src/platforms/nuttx/px4_messages/px4_rc_channels.h | 26 +++ src/platforms/px4_includes.h | 4 + src/platforms/px4_message.h | 77 +++++++++ src/platforms/px4_nodehandle.h | 180 ++++++++++++++------- src/platforms/px4_publisher.h | 41 +++-- src/platforms/px4_subscriber.h | 114 +++++++++---- src/platforms/ros/px4_messages/px4_rc_channels.h | 25 +++ 15 files changed, 496 insertions(+), 172 deletions(-) create mode 100644 src/platforms/nuttx/px4_messages/px4_rc_channels.h create mode 100644 src/platforms/px4_message.h create mode 100644 src/platforms/ros/px4_messages/px4_rc_channels.h 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()) + // _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 * _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(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 * _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 +#include +#include "platforms/px4_message.h" + +#pragma once +namespace px4 +{ + +class px4_rc_channels : + public PX4Message +{ +public: + px4_rc_channels() : + PX4Message() + {} + + px4_rc_channels(rc_channels_s msg) : + PX4Message(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 #include @@ -71,6 +72,9 @@ #include #include #include +#ifdef __cplusplus +#include //XXX +#endif #include #include #include 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 +typedef const struct orb_metatdata* PX4TopicHandle; +#endif + +namespace px4 +{ + +template +class PX4Message +{ + // friend class NodeHandle; +// #if defined(__PX4_ROS) + // template + // 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 #include +#include #else /* includes when building for NuttX */ #include @@ -77,15 +78,25 @@ public: * @param topic Name of the topic * @param fb Callback, executed on receiving a new message */ - template - Subscriber *subscribe(const char *topic, void(*fp)(const M &)) + // template + // Subscriber *subscribe(const char *topic, void(*fp)(const M &)) + // { + // SubscriberBase *sub = new SubscriberROS(std::bind(fp, std::placeholders::_1)); + // ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS::callback, + // (SubscriberROS *)sub); + // ((SubscriberROS *)sub)->set_ros_sub(ros_sub); + // _subs.push_back(sub); + // return (Subscriber *)sub; + // } + template + Subscriber *subscribe(void(*fp)(const typename std::remove_referencedata())>::type &)) { - SubscriberBase *sub = new SubscriberROS(std::bind(fp, std::placeholders::_1)); - ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS::callback, - (SubscriberROS *)sub); - ((SubscriberROS *)sub)->set_ros_sub(ros_sub); + SubscriberBase *sub = new SubscriberROS(std::bind(fp, std::placeholders::_1)); + ros::Subscriber ros_sub = ros::NodeHandle::subscribe((new T())->handle(), kQueueSizeDefault, + &SubscriberROS::callback, (SubscriberROS *)sub); + ((SubscriberROS *)sub)->set_ros_sub(ros_sub); _subs.push_back(sub); - return (Subscriber *)sub; + return (Subscriber *)sub; } /** @@ -93,40 +104,49 @@ public: * @param topic Name of the topic * @param fb Callback, executed on receiving a new message */ - template - Subscriber *subscribe(const char *topic, void(T::*fp)(const M &), T *obj) - { - SubscriberBase *sub = new SubscriberROS(std::bind(fp, obj, std::placeholders::_1)); - ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS::callback, - (SubscriberROS *)sub); - ((SubscriberROS *)sub)->set_ros_sub(ros_sub); - _subs.push_back(sub); - return (Subscriber *)sub; - } + // template + // Subscriber *subscribe(const char *topic, void(T::*fp)(const M &), T *obj) + // { + // SubscriberBase *sub = new SubscriberROS(std::bind(fp, obj, std::placeholders::_1)); + // ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS::callback, + // (SubscriberROS *)sub); + // ((SubscriberROS *)sub)->set_ros_sub(ros_sub); + // _subs.push_back(sub); + // return (Subscriber *)sub; + // } /** * Subscribe with no callback, just the latest value is stored on updates * @param topic Name of the topic */ - template - Subscriber *subscribe(const char *topic) - { - SubscriberBase *sub = new SubscriberROS(); - ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS::callback, - (SubscriberROS *)sub); - ((SubscriberROS *)sub)->set_ros_sub(ros_sub); - _subs.push_back(sub); - return (Subscriber *)sub; - } + // template + // Subscriber *subscribe(const char *topic) + // { + // SubscriberBase *sub = new SubscriberROS(); + // ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, &SubscriberROS::callback, + // (SubscriberROS *)sub); + // ((SubscriberROS *)sub)->set_ros_sub(ros_sub); + // _subs.push_back(sub); + // return (Subscriber *)sub; + // } /** * Advertise topic * @param topic Name of the topic */ - template - Publisher *advertise(const char *topic) + // template + // Publisher *advertise(const char *topic) + // { + // ros::Publisher ros_pub = ros::NodeHandle::advertise(topic, kQueueSizeDefault); + // Publisher *pub = new Publisher(ros_pub); + // _pubs.push_back(pub); + // return pub; + // } + template + Publisher* advertise() + // Publisher *advertise() { - ros::Publisher ros_pub = ros::NodeHandle::advertise(topic, kQueueSizeDefault); + ros::Publisher ros_pub = ros::NodeHandle::advertisedata())>::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 - Subscriber *subscribe(const struct orb_metadata *meta, - std::function callback, - unsigned interval) + // template + // Subscriber *subscribe(const struct orb_metadata *meta, + // std::function callback, + // unsigned interval) + // { + // SubscriberUORBCallback *sub_px4 = new SubscriberUORBCallback(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 *)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 + Subscriber *subscribe(std::functiondata())>::type &)> callback, unsigned interval=10) //XXX interval { - SubscriberUORBCallback *sub_px4 = new SubscriberUORBCallback(meta, interval, callback, &_subs); + const struct orb_metadata * meta = NULL; + uORB::SubscriptionBase * uorb_sub = new uORB::SubscriptionBase(meta, interval); + SubscriberUORBCallback *sub_px4 = new SubscriberUORBCallback(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 *)sub_px4; + return (Subscriber *)sub_px4; } /** @@ -219,29 +261,47 @@ public: * @param interval Minimal interval between data fetches from orb */ - template - Subscriber *subscribe(const struct orb_metadata *meta, - unsigned interval) - { - SubscriberUORB *sub_px4 = new SubscriberUORB(meta, interval, &_subs); + // template + // Subscriber *subscribe(const struct orb_metadata *meta, + // unsigned interval) + // { + // SubscriberUORB *sub_px4 = new SubscriberUORB(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 *)sub_px4; - } + // return (Subscriber *)sub_px4; + // } /** * Advertise topic * @param meta Describes the topic which is advertised */ - template - Publisher *advertise(const struct orb_metadata *meta) + // template + // 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 + 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 _subs; /**< Subcriptions of node */ - List _pubs; /**< Publications of node */ - uORB::SubscriptionNode *_sub_min_interval; /**< Points to the sub wtih the smallest interval + List _subs; /**< Subcriptions of node */ + List _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 #endif +#include + 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 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 - int publish(const M &msg) { _ros_pub.publish(msg); return 0; } + // int publish(const M &msg) { _ros_pub.publish(msg); return 0; } + template + 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 + { 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 *list) : - uORB::PublicationNode(meta, list), - PublisherBase() + // Publisher(const struct orb_metadata *meta, + // List *list) : + // uORB::PublicationNode(meta, list), + // PublisherBase() + // {} + Publisher(uORB::PublicationBase * uorb_pub) : + PublisherBase(), + _uorb_pub(uorb_pub) {} ~Publisher() {}; @@ -109,7 +129,7 @@ public: template 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 +#include #if defined(__PX4_ROS) /* includes when building for ros */ @@ -67,7 +68,7 @@ public: /** * Subscriber class which is used by nodehandle */ -template +template 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 +template class SubscriberROS : - public Subscriber + public Subscriber { friend class NodeHandle; @@ -103,8 +104,8 @@ public: /** * Construct Subscriber by providing a callback function */ - SubscriberROS(std::function cbf) : - Subscriber(), + SubscriberROS(std::functiondata())>::type &)> cbf) : + Subscriber(), _ros_sub(), _cbf(cbf), _msg_current() @@ -114,7 +115,7 @@ public: * Construct Subscriber without a callback function */ SubscriberROS() : - Subscriber(), + Subscriber(), _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_referencedata())>::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 _cbf; /**< Callback that the user provided on the subscription */ - M _msg_current; /**< Current Message value */ + std::functiondata())>::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 +{ +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 +template class SubscriberUORB : - public Subscriber, - public uORB::Subscription + public Subscriber, + 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 *list) : - Subscriber(), - uORB::Subscription(meta, interval, list) + // SubscriberUORB(const struct orb_metadata *meta, + // unsigned interval, + // List *list) : + // Subscriber(), + // uORB::Subscription(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::updated()) { + if (!_uorb_sub->updated()) { /* Topic not updated */ return; } /* get latest data */ - uORB::Subscription::update(); + _uorb_sub->update(get_void_ptr()); }; /* Accessors*/ /** * Get the last message value */ - M get() { return uORB::Subscription::getData(); } + T get() { return (T)(typename std::remove_referencedata())>::type)*_uorb_sub; } /** * Get void pointer to last message value */ - void *get_void_ptr() { return uORB::Subscription::getDataVoidPtr(); } + void *get_void_ptr() { return (void *)(typename std::remove_referencedata())>::type*)_uorb_sub; } + + int getUORBHandle() { return _uorb_sub->getHandle(); } + +protected: + uORB::SubscriptionBase * _uorb_sub; /**< Handle to the subscription */ + typename std::remove_referencedata())>::type getUORBData() { return (typename std::remove_referencedata())>::type)*_uorb_sub; } }; -template +//XXX reduce to one class with overloaded constructor? +template class SubscriberUORBCallback : - public SubscriberUORB + public SubscriberUORB { 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 callback, - List *list) : - SubscriberUORB(meta, interval, list), + // SubscriberUORBCallback(const struct orb_metadata *meta, + // unsigned interval, + // std::function callback, + // List *list) : + // SubscriberUORB(meta, interval, list), + // _callback(callback) + // {} + SubscriberUORBCallback(uORB::SubscriptionBase * uorb_sub, + std::functiondata())>::type &)> callback) : + SubscriberUORB(uorb_sub), _callback(callback) {} @@ -245,13 +289,13 @@ public: */ virtual void update() { - if (!uORB::Subscription::updated()) { + if (!SubscriberUORB::_uorb_sub->updated()) { /* Topic not updated, do not call callback */ return; } /* get latest data */ - uORB::Subscription::update(); + SubscriberUORB::_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::getData()); + _callback(SubscriberUORB::getUORBData()); }; protected: - std::function _callback; /**< Callback handle, + std::functiondata())>::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 +{ +public: + px4_rc_channels() : + PX4Message() + {} + + px4_rc_channels(rc_channels msg) : + PX4Message(msg) + {} + + ~px4_rc_channels() {} + + PX4TopicHandle handle() {return "rc_channels";} +}; + +} -- cgit v1.2.3