fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

added a messageplayer prototype for ros

Browse Source
This commit is contained in:
Thomas Gubler 2015-01-18 18:43:45 +01:00
parent 8121574632
commit 7c3223b860
15 changed files with 496 additions and 172 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

85
CMakeLists.txt
View File

@ -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 ##

3
makefiles/config_px4fmu-v2_multiplatform.mk
View File

@ -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

2
makefiles/config_px4fmu-v2_test.mk
View File

@ -56,7 +56,7 @@ MODULES += systemcmds/ver
# Example modules
#
MODULES += examples/matlab_csv_serial
MODULES += examples/subscriber
#MODULES += examples/subscriber
MODULES += examples/publisher
#

74
msg/templates/msg.h.template
View File

@ -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
};
/**

19
src/examples/publisher/publisher_example.cpp
View File

@ -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();

1
src/examples/publisher/publisher_example.h
View File

@ -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;
};

14
src/examples/subscriber/subscriber_example.cpp
View File

@ -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);
}

3
src/examples/subscriber/subscriber_example.h
View File

@ -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);

26
src/platforms/nuttx/px4_messages/px4_rc_channels.h Normal file
View File

@ -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);}
};
}

4
src/platforms/px4_includes.h
View File

@ -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>

77
src/platforms/px4_message.h Normal file
View File

@ -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;
};
}

180
src/platforms/px4_nodehandle.h
View File

@ -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

41
src/platforms/px4_publisher.h
View File

@ -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
}

114
src/platforms/px4_subscriber.h
View File

@ -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

25
src/platforms/ros/px4_messages/px4_rc_channels.h Normal file
View File

@ -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";}
};
}