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

Merge branch 'dev_ros' into dev_ros_rossharedlib

Browse Source
This commit is contained in:
Thomas Gubler 2014-12-04 13:36:16 +01:00
commit 0b9ef53ac1
14 changed files with 488 additions and 105 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
Makefile
View File

@ -227,12 +227,15 @@ updatesubmodules:
MSG_DIR = $(PX4_BASE)msg/px4_msgs
MSG_TEMPLATE_DIR = $(PX4_BASE)msg/templates
TOPICS_DIR = $(PX4_BASE)src/modules/uORB/topics
TOPICS_TEMPORARY_DIR = $(BUILD_DIR)topics_temporary
.PHONY: generateuorbtopicheaders
generateuorbtopicheaders:
@$(ECHO) "Generating uORB topic headers"
$(Q) ($(PX4_BASE)/Tools/px_generate_uorb_topic_headers.py -d $(MSG_DIR) \
-o $(TOPICS_DIR) -e $(MSG_TEMPLATE_DIR))
-o $(TOPICS_DIR) -e $(MSG_TEMPLATE_DIR) -t $(TOPICS_TEMPORARY_DIR))
# clean up temporary files
$(Q) (rm -r $(TOPICS_TEMPORARY_DIR))
#
# Testing targets

121
Tools/px_generate_uorb_topic_headers.py
View File

@ -39,6 +39,8 @@ message files
"""
from __future__ import print_function
import os
import shutil
import filecmp
import argparse
import genmsg.template_tools
@ -55,44 +57,93 @@ package = 'px4'
def convert_file(filename, outputdir, templatedir, includepath):
"""
Converts a single .msg file to a uorb header
"""
print("Generating uORB headers from {0}".format(filename))
genmsg.template_tools.generate_from_file(filename,
package,
outputdir,
templatedir,
includepath,
msg_template_map,
srv_template_map)
"""
Converts a single .msg file to a uorb header
"""
print("Generating uORB headers from {0}".format(filename))
genmsg.template_tools.generate_from_file(filename,
package,
outputdir,
templatedir,
includepath,
msg_template_map,
srv_template_map)
def convert_dir(inputdir, outputdir, templatedir):
"""
Converts all .msg files in inputdir to uORB header files
"""
includepath = incl_default + [':'.join([package, inputdir])]
for f in os.listdir(inputdir):
fn = os.path.join(inputdir, f)
if os.path.isfile(fn):
convert_file(fn, outputdir, templatedir, includepath)
"""
Converts all .msg files in inputdir to uORB header files
"""
includepath = incl_default + [':'.join([package, inputdir])]
for f in os.listdir(inputdir):
fn = os.path.join(inputdir, f)
if os.path.isfile(fn):
convert_file(
fn,
outputdir,
templatedir,
includepath)
def copy_changed(inputdir, outputdir):
"""
Copies files from inputdir to outputdir if they don't exist in
ouputdir or if their content changed
"""
for f in os.listdir(inputdir):
fni = os.path.join(inputdir, f)
if os.path.isfile(fni):
# Check if f exists in outpoutdir, copy the file if not
fno = os.path.join(outputdir, f)
if not os.path.isfile(fno):
shutil.copy(fni, fno)
print("{0}: new header file".format(f))
continue
# The file exists in inputdir and outputdir
# only copy if contents do not match
if not filecmp.cmp(fni, fno):
shutil.copy(fni, fno)
print("{0}: updated".format(f))
continue
print("{0}: unchanged".format(f))
def convert_dir_save(inputdir, outputdir, templatedir, temporarydir):
"""
Converts all .msg files in inputdir to uORB header files
Unchanged existing files are not overwritten.
"""
# Create new headers in temporary output directory
convert_dir(inputdir, temporarydir, templatedir)
# Copy changed headers from temporary dir to output dir
copy_changed(temporarydir, outputdir)
if __name__ == "__main__":
parser = argparse.ArgumentParser(
description='Convert msg files to uorb headers')
parser.add_argument('-d', dest='dir', help='directory with msg files')
parser.add_argument('-f', dest='file',
help="files to convert (use only without -d)",
nargs="+")
parser.add_argument('-e', dest='templatedir',
help='directory with template files',)
parser.add_argument('-o', dest='outputdir',
help='output directory for header files')
args = parser.parse_args()
parser = argparse.ArgumentParser(
description='Convert msg files to uorb headers')
parser.add_argument('-d', dest='dir', help='directory with msg files')
parser.add_argument('-f', dest='file',
help="files to convert (use only without -d)",
nargs="+")
parser.add_argument('-e', dest='templatedir',
help='directory with template files',)
parser.add_argument('-o', dest='outputdir',
help='output directory for header files')
parser.add_argument('-t', dest='temporarydir',
help='temporary directory')
args = parser.parse_args()
if args.file is not None:
for f in args.file:
convert_file(f, args.outputdir, args.templatedir, incl_default)
elif args.dir is not None:
convert_dir(args.dir, args.outputdir, args.templatedir)
if args.file is not None:
for f in args.file:
convert_file(
f,
args.outputdir,
args.templatedir,
incl_default)
elif args.dir is not None:
convert_dir_save(
args.dir,
args.outputdir,
args.templatedir,
args.temporarydir)

6
makefiles/setup.mk
View File

@ -37,13 +37,14 @@
# Some useful paths.
#
# Note that in general we always keep directory paths with the separator
# at the end, and join paths without explicit separators. This reduces
# at the end, and join paths without explicit separators. This reduces
# the number of duplicate slashes we have lying around in paths,
# and is consistent with joining the results of $(dir) and $(notdir).
#
export PX4_INCLUDE_DIR = $(abspath $(PX4_BASE)/src/include)/
export PX4_MODULE_SRC = $(abspath $(PX4_BASE)/src)/
export PX4_LIB_DIR = $(abspath $(PX4_BASE)/src/lib)/
export PX4_PLATFORMS_DIR = $(abspath $(PX4_BASE)/src/platforms)/
export PX4_MK_DIR = $(abspath $(PX4_BASE)/makefiles)/
export NUTTX_SRC = $(abspath $(PX4_BASE)/NuttX/nuttx)/
export MAVLINK_SRC = $(abspath $(PX4_BASE)/mavlink/include/mavlink/v1.0)/
@ -61,7 +62,8 @@ export ARCHIVE_DIR = $(abspath $(PX4_BASE)/Archives)/
export INCLUDE_DIRS := $(PX4_MODULE_SRC) \
$(PX4_MODULE_SRC)/modules/ \
$(PX4_INCLUDE_DIR) \
$(PX4_LIB_DIR)
$(PX4_LIB_DIR) \
$(PX4_PLATFORMS_DIR)
#
# Tools

3
src/examples/subscriber/module.mk
View File

@ -37,6 +37,7 @@
MODULE_COMMAND = subscriber
SRCS = subscriber.cpp
SRCS = subscriber.cpp \
subscriber_params.c
MODULE_STACKSIZE = 2400

15
src/examples/subscriber/subscriber.cpp
View File

@ -26,6 +26,7 @@
*/
#include <px4.h>
#include "subscriber_params.h"
using namespace px4;
@ -74,6 +75,18 @@ PX4_MAIN_FUNCTION(subscriber) {
*/
px4::NodeHandle n;
/* Define parameters */
px4_param_t p_sub_interv = PX4_PARAM_INIT(SUB_INTERV);
px4_param_t p_test_float = PX4_PARAM_INIT(SUB_TESTF);
/* Read the parameter back for testing */
int32_t sub_interval;
float test_float;
PX4_PARAM_GET(p_sub_interv, &sub_interval);
PX4_INFO("Param SUB_INTERV = %d", sub_interval);
PX4_PARAM_GET(p_test_float, &test_float);
PX4_INFO("Param SUB_TESTF = %.3f", (double)test_float);
/**
* The subscribe() call is how you tell ROS that you want to receive messages
* on a given topic. This invokes a call to the ROS
@ -89,7 +102,7 @@ PX4_MAIN_FUNCTION(subscriber) {
* is the number of messages that will be buffered up before beginning to throw
* away the oldest ones.
*/
PX4_SUBSCRIBE(n, rc_channels, rc_channels_callback, 100);
PX4_SUBSCRIBE(n, rc_channels, rc_channels_callback, sub_interval);
PX4_SUBSCRIBE(n, rc_channels, rc_channels_callback2, 1000);
PX4_SUBSCRIBE(n, rc_channels, RCHandler::callback, rchandler, 1000);
PX4_INFO("subscribed");

56
src/examples/subscriber/subscriber_params.c Normal file
View File

@ -0,0 +1,56 @@
/****************************************************************************
*
* Copyright (c) 2013, 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 subscriber_params.c
* Parameters for the subscriber example
*
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#include <px4_defines.h>
#include "subscriber_params.h"
/**
* Interval of one subscriber in the example in ms
*
* @group Subscriber Example
*/
PX4_PARAM_DEFINE_INT32(SUB_INTERV);
/**
* Float Demonstration Parameter in the Example
*
* @group Subscriber Example
*/
PX4_PARAM_DEFINE_FLOAT(SUB_TESTF);

43
src/examples/subscriber/subscriber_params.h Normal file
View File

@ -0,0 +1,43 @@
/****************************************************************************
*
* Copyright (c) 2013, 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 subscriber_params.h
* Default parameters for the subscriber example
*
* @author Thomas Gubler <thomasgubler@gmail.com>
*/
#pragma once
#define PARAM_SUB_INTERV_DEFAULT 100
#define PARAM_SUB_TESTF_DEFAULT 3.14f

21
src/include/px4.h
View File

@ -39,26 +39,7 @@
#pragma once
#include <stdbool.h>
#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__))
/*
* Building for running within the ROS environment
*/
#include "ros/ros.h"
#include "px4/rc_channels.h"
#else
/*
* Building for NuttX
*/
#include <nuttx/config.h>
#include <uORB/uORB.h>
#include <uORB/topics/rc_channels.h>
#include <systemlib/err.h>
#endif
#include "../platforms/px4_includes.h"
#include "../platforms/px4_defines.h"
#include "../platforms/px4_middleware.h"
#include "../platforms/px4_nodehandle.h"

67
src/platforms/px4_defines.h
View File

@ -38,36 +38,99 @@
*/
#pragma once
/* Get the name of the default value fiven the param name */
#define PX4_PARAM_DEFAULT_VALUE_NAME(_name) PARAM_##_name##_DEFAULT
/* Shortcuts to define parameters when the default value is defined according to PX4_PARAM_DEFAULT_VALUE_NAME */
#define PX4_PARAM_DEFINE_INT32(_name) PARAM_DEFINE_INT32(_name, PX4_PARAM_DEFAULT_VALUE_NAME(_name))
#define PX4_PARAM_DEFINE_FLOAT(_name) PARAM_DEFINE_FLOAT(_name, PX4_PARAM_DEFAULT_VALUE_NAME(_name))
#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__))
/*
* Building for running within the ROS environment
*/
#define __EXPORT
#include "ros/ros.h"
/* Main entry point */
#define PX4_MAIN_FUNCTION(_prefix) int main(int argc, char **argv)
/* Print/output wrappers */
#define PX4_WARN ROS_WARN
#define PX4_INFO ROS_INFO
/* Topic Handle */
#define PX4_TOPIC(_name) #_name
/* Topic type */
#define PX4_TOPIC_T(_name) _name
/* Subscribe and providing a class method as callback (do not use directly, use PX4_SUBSCRIBE instead) */
#define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _obj, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), &_cbf, &_obj);
/* Subscribe and providing a function as callback (do not use directly, use PX4_SUBSCRIBE instead) */
#define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), _cbf);
/* Parameter handle datatype */
typedef const char *px4_param_t;
/* Helper fucntions to set ROS params, only int and float supported */
static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, int value)
{
ros::param::set(name, value);
return (px4_param_t)name;
};
static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, float value)
{
ros::param::set(name, value);
return (px4_param_t)name;
};
/* Initialize a param, in case of ROS the parameter is sent to the parameter server here */
#define PX4_PARAM_INIT(_name) PX4_ROS_PARAM_SET(#_name, PX4_PARAM_DEFAULT_VALUE_NAME(_name))
/* Get value of parameter */
#define PX4_PARAM_GET(_handle, _destpt) ros::param::get(_handle, *_destpt)
#else
/*
* Building for NuttX
*/
#include <platforms/px4_includes.h>
/* Main entry point */
#define PX4_MAIN_FUNCTION(_prefix) extern "C" __EXPORT int _prefix##_main(int argc, char *argv[])
#define PX4_WARN warnx
/* Print/output wrappers */
#define PX4_WARN warnx
#define PX4_INFO warnx
/* Topic Handle */
#define PX4_TOPIC(_name) ORB_ID(_name)
/* Topic type */
#define PX4_TOPIC_T(_name) _name##_s
/* Subscribe and providing a class method as callback (do not use directly, use PX4_SUBSCRIBE instead) */
#define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _obj, _interval) _nodehandle.subscribe<PX4_TOPIC_T(_name)>(PX4_TOPIC(_name), std::bind(&_cbf, _obj, std::placeholders::_1), _interval)
/* Subscribe and providing a function as callback (do not use directly, use PX4_SUBSCRIBE instead) */
#define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe<PX4_TOPIC_T(_name)>(PX4_TOPIC(_name), std::bind(&_cbf, std::placeholders::_1), _interval)
/* Parameter handle datatype */
typedef param_t px4_param_t;
/* Initialize a param, get param handle */
#define PX4_PARAM_INIT(_name) param_find(#_name)
/* Get value of parameter */
#define PX4_PARAM_GET(_handle, _destpt) param_get(_handle, _destpt)
#endif
/* Overload the PX4_SUBSCRIBE macro to suppport methods and pure functions as callback */
/* Shortcut for subscribing to topics
* Overload the PX4_SUBSCRIBE macro to suppport methods and pure functions as callback
*/
#define PX4_GET_SUBSCRIBE(_1, _2, _3, _4, _5, NAME, ...) NAME
#define PX4_SUBSCRIBE(...) PX4_GET_SUBSCRIBE(__VA_ARGS__, PX4_SUBSCRIBE_CBMETH, PX4_SUBSCRIBE_CBFUNC)(__VA_ARGS__)
/* shortcut for advertising topics */
#define PX4_ADVERTISE(_nodehandle, _name) _nodehandle.advertise<PX4_TOPIC_T(_name)>(PX4_TOPIC(_name))
/* wrapper for 2d matrices */

61
src/platforms/px4_includes.h Normal file
View File

@ -0,0 +1,61 @@
/****************************************************************************
*
* 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_includes.h
*
* Includes headers depending on the build target
*/
#pragma once
#include <stdbool.h>
#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__))
/*
* Building for running within the ROS environment
*/
#include "ros/ros.h"
#include "px4/rc_channels.h"
#else
/*
* Building for NuttX
*/
#include <nuttx/config.h>
#include <uORB/uORB.h>
#include <uORB/topics/rc_channels.h>
#include <systemlib/err.h>
#include <systemlib/param/param.h>
#endif

13
src/platforms/px4_middleware.h
View File

@ -50,9 +50,15 @@ __EXPORT void init(int argc, char *argv[], const char *process_name);
__EXPORT uint64_t get_time_micros();
#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__))
/**
* Returns true if the app/task should continue to run
*/
bool ok() { return ros::ok(); }
#else
extern bool task_should_exit;
/**
* Returns true if the app/task should continue to run
*/
bool ok() { return !task_should_exit; }
#endif
@ -60,8 +66,15 @@ class Rate
{
public:
/**
* Construct the Rate object and set rate
* @param rate_hz rate from which sleep time is calculated in Hz
*/
explicit Rate(unsigned rate_hz) { sleep_interval = 1e6 / rate_hz; }
/**
* Sleep for 1/rate_hz s
*/
void sleep() { usleep(sleep_interval); }
private:

106
src/platforms/px4_nodehandle.h
View File

@ -55,7 +55,6 @@
namespace px4
{
//XXX create abstract base class
#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__))
class NodeHandle :
private ros::NodeHandle
@ -67,42 +66,67 @@ public:
_pubs()
{}
~NodeHandle() {
~NodeHandle()
{
//XXX empty lists
};
/* Constructor with callback to function */
/**
* Subscribe with callback to function
* @param topic Name of the topic
* @param fb Callback, executed on receiving a new message
*/
template<typename M>
Subscriber * subscribe(const char *topic, void(*fp)(M)) {
Subscriber *subscribe(const char *topic, void(*fp)(M))
{
ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, fp);
Subscriber * sub = new Subscriber(ros_sub);
_subs.push_back(sub);
return sub;
}
/* Constructor with callback to class method */
template<typename M, typename T>
Subscriber * subscribe(const char *topic, void(T::*fp)(M), T *obj) {
ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, fp, obj);
Subscriber * sub = new Subscriber(ros_sub);
Subscriber *sub = new Subscriber(ros_sub);
_subs.push_back(sub);
return sub;
}
/**
* Subscribe with callback to class method
* @param topic Name of the topic
* @param fb Callback, executed on receiving a new message
*/
template<typename M, typename T>
Subscriber *subscribe(const char *topic, void(T::*fp)(M), T *obj)
{
ros::Subscriber ros_sub = ros::NodeHandle::subscribe(topic, kQueueSizeDefault, fp, obj);
Subscriber *sub = new Subscriber(ros_sub);
_subs.push_back(sub);
return sub;
}
/**
* Advertise topic
* @param topic Name of the topic
*/
template<typename M>
Publisher * advertise(const char *topic) {
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;
}
/**
* Calls all callback waiting to be called
*/
void spinOnce() { ros::spinOnce(); }
/**
* Keeps calling callbacks for incomming messages, returns when module is terminated
*/
void spin() { ros::spin(); }
void spinOnce() { ros::spinOnce(); }
private:
static const uint32_t kQueueSizeDefault = 1000;
std::list<Subscriber*> _subs;
std::list<Publisher*> _pubs;
static const uint32_t kQueueSizeDefault = 1000; /**< Size of queue for ROS */
std::list<Subscriber *> _subs; /**< Subcriptions of node */
std::list<Publisher *> _pubs; /**< Publications of node */
};
#else
class __EXPORT NodeHandle
@ -116,27 +140,45 @@ public:
~NodeHandle() {};
/**
* 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 M>
Subscriber * subscribe(const struct orb_metadata *meta,
std::function<void(const M&)> callback,
unsigned interval) {
Subscriber *subscribe(const struct orb_metadata *meta,
std::function<void(const M &)> callback,
unsigned interval)
{
SubscriberPX4<M> *sub_px4 = new SubscriberPX4<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*)sub_px4;
return (Subscriber *)sub_px4;
}
/**
* Advertise topic
* @param meta Describes the topic which is advertised
*/
template<typename M>
Publisher * advertise(const struct orb_metadata *meta) {
Publisher *advertise(const struct orb_metadata *meta)
{
//XXX
Publisher * pub = new Publisher(meta, &_pubs);
Publisher *pub = new Publisher(meta, &_pubs);
return pub;
}
void spinOnce() {
/**
* Calls all callback waiting to be called
*/
void spinOnce()
{
/* Loop through subscriptions, call callback for updated subscriptions */
uORB::SubscriptionNode *sub = _subs.getHead();
int count = 0;
@ -152,9 +194,14 @@ public:
}
}
void spin() {
/**
* Keeps calling callbacks for incomming messages, returns when module is terminated
*/
void spin()
{
while (ok()) {
const int timeout_ms = 100;
/* Only continue in the loop if the nodehandle has subscriptions */
if (_sub_min_interval == nullptr) {
usleep(timeout_ms * 1000);
@ -165,6 +212,7 @@ public:
struct pollfd pfd;
pfd.fd = _sub_min_interval->getHandle();
pfd.events = POLLIN;
if (poll(&pfd, 1, timeout_ms) <= 0) {
/* timed out */
continue;
@ -175,9 +223,9 @@ public:
}
private:
static const uint16_t kMaxSubscriptions = 100;
List<uORB::SubscriptionNode*> _subs;
List<uORB::PublicationNode*> _pubs;
uORB::SubscriptionNode* _sub_min_interval; /**< Points to the sub wtih the smallest interval
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
of all Subscriptions in _subs*/
};
#endif

35
src/platforms/px4_publisher.h
View File

@ -52,33 +52,54 @@ namespace px4
class Publisher
{
public:
/**
* Construct Publisher by providing a ros::Publisher
* @param ros_pub the ros publisher which will be used to perform the publications
*/
Publisher(ros::Publisher ros_pub) : _ros_pub(ros_pub)
{}
~Publisher() {};
/** 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; }
private:
ros::Publisher _ros_pub;
ros::Publisher _ros_pub; /**< Handle to the ros publisher */
};
#else
class Publisher :
public uORB::PublicationNode
{
public:
/**
* Construct Publisher by providing orb meta data
* @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) :
List<uORB::PublicationNode *> *list) :
uORB::PublicationNode(meta, list)
{}
~Publisher() {};
/** Publishes msg
* @param msg the message which is published to the topic
*/
template<typename M>
int publish(const M &msg) {
uORB::PublicationBase::update((void*)&msg);
int publish(const M &msg)
{
uORB::PublicationBase::update((void *)&msg);
return 0;
}
void update() {
//XXX list traversal callback, needed?
} ;
/**
* Empty callback for list traversal
*/
void update() {} ;
};
#endif
}

41
src/platforms/px4_subscriber.h
View File

@ -54,15 +54,24 @@ namespace px4
class Subscriber
{
public:
/**
* Construct Subscriber by providing a ros::Subscriber
* @param ros_sub the ros subscriber which will be used to perform the publications
*/
Subscriber(ros::Subscriber ros_sub) :
_ros_sub(ros_sub)
{}
~Subscriber() {};
private:
ros::Subscriber _ros_sub;
ros::Subscriber _ros_sub; /**< Handle to ros subscriber */
};
#else
// typedef std::function<void(int)> CallbackFunction;
/**
* Subscriber class which is used by nodehandle when building for NuttX
*/
class Subscriber
{
public:
@ -71,24 +80,41 @@ public:
private:
};
/**
* Subscriber class that is templated with the uorb subscription message type
*/
template<typename M>
class SubscriberPX4 :
public Subscriber,
public uORB::Subscription<M>
{
public:
/**
* Construct SubscriberPX4 by providing orb meta data
* @param meta orb metadata for the topic which is used
* @param callback Callback, executed on receiving a new message
* @param interval Minimal interval between calls to callback
* @param list subscriber is added to this list
*/
SubscriberPX4(const struct orb_metadata *meta,
unsigned interval,
std::function<void(const M&)> callback,
List<uORB::SubscriptionNode *> * list) :
unsigned interval,
std::function<void(const M &)> callback,
List<uORB::SubscriptionNode *> *list) :
Subscriber(),
uORB::Subscription<M>(meta, interval, list),
_callback(callback)
//XXX store callback
{}
~SubscriberPX4() {};
void update() {
/**
* Update Subscription
* Invoked by the list traversal in NodeHandle::spinOnce
* If new data is available the callback is called
*/
void update()
{
if (!uORB::Subscription<M>::updated()) {
/* Topic not updated, do not call callback */
return;
@ -102,7 +128,8 @@ public:
};
private:
std::function<void(const M&)> _callback;
std::function<void(const M &)> _callback; /**< Callback handle,
called when new data is available */
};
#endif