dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Restructuring of generic middleware support files, wrapping of the main ROS calls, skeletons for publishers / subscribers

This commit is contained in:
Lorenz Meier 2014-11-09 11:57:34 +01:00
parent 1e9f431cf1
commit f36f54c621
20 changed files with 851 additions and 167 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
CMakeLists.txt
View File

@ -80,10 +80,10 @@ generate_messages(
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
INCLUDE_DIRS include
# LIBRARIES px4
# CATKIN_DEPENDS roscpp rospy std_msgs
# DEPENDS system_lib
INCLUDE_DIRS src/include
LIBRARIES px4
CATKIN_DEPENDS roscpp rospy std_msgs
DEPENDS system_lib
CATKIN_DEPENDS message_runtime
)
@ -93,15 +93,22 @@ catkin_package(
## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(include)
include_directories(
${catkin_INCLUDE_DIRS}
src/platforms
src/include
)
## Declare a cpp library
# add_library(px4
# src/${PROJECT_NAME}/px4test.cpp # src/platforms/ros/ros.cpp
# )
add_library(px4
src/platforms/ros/px4_ros_impl.cpp
src/platforms/ros/px4_publisher.cpp
src/platforms/ros/px4_subscriber.cpp
)
target_link_libraries(px4
${catkin_LIBRARIES}
)
## Declare a test publisher
add_executable(publisher src/examples/publisher/publisher.cpp)
@ -113,6 +120,7 @@ add_dependencies(publisher px4_generate_messages_cpp)
## Specify libraries to link a library or executable target against
target_link_libraries(publisher
${catkin_LIBRARIES}
px4
)
## Declare a test subscriber
@ -125,6 +133,7 @@ add_dependencies(subscriber px4_generate_messages_cpp)
## Specify libraries to link a library or executable target against
target_link_libraries(subscriber
${catkin_LIBRARIES}
px4
)
#############
@ -142,11 +151,11 @@ target_link_libraries(subscriber
# )
## Mark executables and/or libraries for installation
# install(TARGETS px4 mc_attitude_control
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
install(TARGETS px4 publisher subscriber
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/

13
makefiles/config_px4fmu-v2_test.mk
View File

@ -34,9 +34,11 @@ MODULES += systemcmds/mtd
MODULES += systemcmds/ver
#
# Testing modules
# Example modules
#
MODULES += examples/matlab_csv_serial
MODULES += examples/subscriber
MODULES += examples/publisher
#
# Library modules
@ -44,16 +46,15 @@ MODULES += examples/matlab_csv_serial
MODULES += modules/systemlib
MODULES += modules/systemlib/mixer
MODULES += modules/uORB
LIBRARIES += lib/mathlib/CMSIS
MODULES += lib/mathlib
MODULES += lib/mathlib/math/filter
MODULES += lib/conversion
#
# Libraries
#
LIBRARIES += lib/mathlib/CMSIS
MODULES += platforms/nuttx
#
# Tests
#
MODULES += modules/unit_test
MODULES += modules/mavlink/mavlink_tests
MODULES += modules/commander/commander_tests

42
src/examples/publisher/module.mk Normal file
View File

@ -0,0 +1,42 @@
############################################################################
#
# 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.
#
############################################################################
#
# Publisher Example Application
#
MODULE_COMMAND = publisher
SRCS = publisher.cpp
MODULE_STACKSIZE = 1200

140
src/examples/publisher/publisher.cpp
View File

@ -26,112 +26,70 @@
*/
#include <px4.h>
// %EndTag(ROS_HEADER)%
// %Tag(MSG_HEADER)%
#include "std_msgs/String.h"
// %EndTag(MSG_HEADER)%
#include <sstream>
#include <px4/rc_channels.h>
#include <sstream>
/**
* This tutorial demonstrates simple sending of messages over the ROS system.
* This tutorial demonstrates simple sending of messages over the PX4 middleware system.
*/
int main(int argc, char **argv)
{
/**
* The ros::init() function needs to see argc and argv so that it can perform
* any ROS arguments and name remapping that were provided at the command line. For programmatic
* remappings you can use a different version of init() which takes remappings
* directly, but for most command-line programs, passing argc and argv is the easiest
* way to do it. The third argument to init() is the name of the node.
*
* You must call one of the versions of ros::init() before using any other
* part of the ROS system.
*/
// %Tag(INIT)%
ros::init(argc, argv, "px4_publisher");
// %EndTag(INIT)%
/**
* NodeHandle is the main access point to communications with the ROS system.
* The first NodeHandle constructed will fully initialize this node, and the last
* NodeHandle destructed will close down the node.
*/
// %Tag(NODEHANDLE)%
ros::NodeHandle n;
// %EndTag(NODEHANDLE)%
px4::init(argc, argv, "px4_publisher");
/**
* The advertise() function is how you tell ROS that you want to
* publish on a given topic name. This invokes a call to the ROS
* master node, which keeps a registry of who is publishing and who
* is subscribing. After this advertise() call is made, the master
* node will notify anyone who is trying to subscribe to this topic name,
* and they will in turn negotiate a peer-to-peer connection with this
* node. advertise() returns a Publisher object which allows you to
* publish messages on that topic through a call to publish(). Once
* all copies of the returned Publisher object are destroyed, the topic
* will be automatically unadvertised.
*
* The second parameter to advertise() is the size of the message queue
* used for publishing messages. If messages are published more quickly
* than we can send them, the number here specifies how many messages to
* buffer up before throwing some away.
*/
// %Tag(PUBLISHER)%
ros::Publisher rc_channels_pub = n.advertise<px4::rc_channels>("rc_channels", 1000);
// %EndTag(PUBLISHER)%
ros::NodeHandle n;
// %Tag(LOOP_RATE)%
ros::Rate loop_rate(10);
// %EndTag(LOOP_RATE)%
/**
* The advertise() function is how you tell ROS that you want to
* publish on a given topic name. This invokes a call to the ROS
* master node, which keeps a registry of who is publishing and who
* is subscribing. After this advertise() call is made, the master
* node will notify anyone who is trying to subscribe to this topic name,
* and they will in turn negotiate a peer-to-peer connection with this
* node. advertise() returns a Publisher object which allows you to
* publish messages on that topic through a call to publish(). Once
* all copies of the returned Publisher object are destroyed, the topic
* will be automatically unadvertised.
*
* The second parameter to advertise() is the size of the message queue
* used for publishing messages. If messages are published more quickly
* than we can send them, the number here specifies how many messages to
* buffer up before throwing some away.
*/
ros::Publisher rc_channels_pub = n.advertise<px4::rc_channels>("rc_channels", 1000);
/**
* A count of how many messages we have sent. This is used to create
* a unique string for each message.
*/
// %Tag(ROS_OK)%
int count = 0;
while (ros::ok())
{
// %EndTag(ROS_OK)%
/**
* This is a message object. You stuff it with data, and then publish it.
*/
// %Tag(FILL_MESSAGE)%
px4::rc_channels msg;
px4::Rate loop_rate(10);
ros::Time time = ros::Time::now();
msg.timestamp_last_valid = time.sec * 1e6 + time.nsec;
// %EndTag(FILL_MESSAGE)%
/**
* A count of how many messages we have sent. This is used to create
* a unique string for each message.
*/
int count = 0;
// %Tag(ROSCONSOLE)%
px4_warnx("%lu", msg.timestamp_last_valid);
// %EndTag(ROSCONSOLE)%
while (px4::ok()) {
/**
* This is a message object. You stuff it with data, and then publish it.
*/
px4::rc_channels msg;
/**
* The publish() function is how you send messages. The parameter
* is the message object. The type of this object must agree with the type
* given as a template parameter to the advertise<>() call, as was done
* in the constructor above.
*/
// %Tag(PUBLISH)%
rc_channels_pub.publish(msg);
// %EndTag(PUBLISH)%
msg.timestamp_last_valid = px4::get_time_micros();
PX4_INFO("%lu", msg.timestamp_last_valid);
// %Tag(SPINONCE)%
ros::spinOnce();
// %EndTag(SPINONCE)%
/**
* The publish() function is how you send messages. The parameter
* is the message object. The type of this object must agree with the type
* given as a template parameter to the advertise<>() call, as was done
* in the constructor above.
*/
rc_channels_pub.publish(msg);
// %Tag(RATE_SLEEP)%
loop_rate.sleep();
// %EndTag(RATE_SLEEP)%
++count;
}
px4::spin_once();
loop_rate.sleep();
++count;
}
return 0;
return 0;
}
// %EndTag(FULLTEXT)%

42
src/examples/subscriber/module.mk Normal file
View File

@ -0,0 +1,42 @@
############################################################################
#
# 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.
#
############################################################################
#
# Subscriber Example Application
#
MODULE_COMMAND = subscriber
SRCS = subscriber.cpp
MODULE_STACKSIZE = 1200

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

@ -25,70 +25,63 @@
* POSSIBILITY OF SUCH DAMAGE.
*/
// %Tag(FULLTEXT)%
#include "ros/ros.h"
#include "std_msgs/String.h"
#include <px4.h>
#include "px4/rc_channels.h"
/**
* This tutorial demonstrates simple receipt of messages over the ROS system.
* This tutorial demonstrates simple receipt of messages over the PX4 middleware system.
*/
// %Tag(CALLBACK)%
void rc_channels_callback(const px4::rc_channels::ConstPtr& msg)
void rc_channels_callback(const px4::rc_channels::ConstPtr &msg)
{
ROS_INFO("I heard: [%lu]", msg->timestamp_last_valid);
PX4_INFO("I heard: [%lu]", msg->timestamp_last_valid);
}
// %EndTag(CALLBACK)%
PX4_MAIN_FUNCTION(subscriber)
int main(int argc, char **argv)
{
/**
* The ros::init() function needs to see argc and argv so that it can perform
* any ROS arguments and name remapping that were provided at the command line. For programmatic
* remappings you can use a different version of init() which takes remappings
* directly, but for most command-line programs, passing argc and argv is the easiest
* way to do it. The third argument to init() is the name of the node.
*
* You must call one of the versions of ros::init() before using any other
* part of the ROS system.
*/
ros::init(argc, argv, "listener");
/**
* The ros::init() function needs to see argc and argv so that it can perform
* any ROS arguments and name remapping that were provided at the command line. For programmatic
* remappings you can use a different version of init() which takes remappings
* directly, but for most command-line programs, passing argc and argv is the easiest
* way to do it. The third argument to init() is the name of the node.
*
* You must call one of the versions of px4::init() before using any other
* part of the PX4/ ROS system.
*/
px4::init(argc, argv, "listener");
/**
* NodeHandle is the main access point to communications with the ROS system.
* The first NodeHandle constructed will fully initialize this node, and the last
* NodeHandle destructed will close down the node.
*/
ros::NodeHandle n;
/**
* NodeHandle is the main access point to communications with the ROS system.
* The first NodeHandle constructed will fully initialize this node, and the last
* NodeHandle destructed will close down the node.
*/
ros::NodeHandle n;
/**
* 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
* master node, which keeps a registry of who is publishing and who
* is subscribing. Messages are passed to a callback function, here
* called chatterCallback. subscribe() returns a Subscriber object that you
* must hold on to until you want to unsubscribe. When all copies of the Subscriber
* object go out of scope, this callback will automatically be unsubscribed from
* this topic.
*
* The second parameter to the subscribe() function is the size of the message
* queue. If messages are arriving faster than they are being processed, this
* is the number of messages that will be buffered up before beginning to throw
* away the oldest ones.
*/
// %Tag(SUBSCRIBER)%
ros::Subscriber sub = n.subscribe("rc_channels", 1000, rc_channels_callback);
// %EndTag(SUBSCRIBER)%
/**
* 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
* master node, which keeps a registry of who is publishing and who
* is subscribing. Messages are passed to a callback function, here
* called chatterCallback. subscribe() returns a Subscriber object that you
* must hold on to until you want to unsubscribe. When all copies of the Subscriber
* object go out of scope, this callback will automatically be unsubscribed from
* this topic.
*
* The second parameter to the subscribe() function is the size of the message
* queue. If messages are arriving faster than they are being processed, this
* is the number of messages that will be buffered up before beginning to throw
* away the oldest ones.
*/
ros::Subscriber sub = n.subscribe("rc_channels", 1000, rc_channels_callback);
/**
* ros::spin() will enter a loop, pumping callbacks. With this version, all
* callbacks will be called from within this thread (the main one). ros::spin()
* will exit when Ctrl-C is pressed, or the node is shutdown by the master.
*/
// %Tag(SPIN)%
ros::spin();
// %EndTag(SPIN)%
/**
* px4::spin() will enter a loop, pumping callbacks. With this version, all
* callbacks will be called from within this thread (the main one). px4::spin()
* will exit when Ctrl-C is pressed, or the node is shutdown by the master.
*/
px4::spin();
return 0;
return 0;
}
// %EndTag(FULLTEXT)%

15
include/px4.h → src/include/px4.h
View File

@ -37,17 +37,24 @@
* Main system header with common convenience functions
*/
#pragma once
#include <stdbool.h>
#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__))
/*
* Building for running within the ROS environment
*/
#include "ros/ros.h"
#define px4_warnx ROS_WARN
#define px4_infox ROS_INFO
#define PX4_WARN ROS_WARN
#define PX4_INFO ROS_INFO
#else
/*
* Building for NuttX
*/
#define px4_warnx warnx
#define px4_infox warnx
#define PX4_WARN warnx
#define PX4_INFO warnx
#endif
#include "../platforms/px4_defines.h"
#include "../platforms/px4_middleware.h"

0
platforms/empty.c → src/platforms/empty.c
View File

42
src/platforms/nuttx/module.mk Normal file
View File

@ -0,0 +1,42 @@
############################################################################
#
# 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.
#
############################################################################
#
# NuttX / uORB adapter library
#
SRCS = px4_nuttx_impl.cpp \
px4_publisher.cpp \
px4_subscriber.cpp
MAXOPTIMIZATION = -Os

75
src/platforms/nuttx/px4_nuttx_impl.cpp Normal file
View File

@ -0,0 +1,75 @@
/****************************************************************************
*
* 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_nuttx_impl.cpp
*
* PX4 Middleware Wrapper NuttX Implementation
*/
#include <px4.h>
extern bool task_should_exit;
namespace px4
{
void init(int argc, char *argv[], const char *process_name)
{
px4_warn("process: %s", process_name);
return 0;
}
uint64_t get_time_micros()
{
return hrt_absolute_time();
}
bool ok()
{
return !task_should_exit;
}
void spin_once()
{
// XXX check linked list of topics with orb_check() here
}
void spin()
{
// XXX block waiting for updated topics here
}
}

40
src/platforms/nuttx/px4_publisher.cpp Normal file
View File

@ -0,0 +1,40 @@
/****************************************************************************
*
* 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_publisher.cpp
*
* PX4 Middleware Wrapper for Publisher
*/

40
src/platforms/nuttx/px4_subscriber.cpp Normal file
View File

@ -0,0 +1,40 @@
/****************************************************************************
*
* 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_subscriber.cpp
*
* PX4 Middleware Wrapper Subscriber
*/

51
src/platforms/px4_defines.h Normal file
View File

@ -0,0 +1,51 @@
/****************************************************************************
*
* 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_defines.h
*
* Generally used magic defines
*/
#pragma once
#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__))
/*
* Building for running within the ROS environment
*/
#define __EXPORT
#define PX4_MAIN_FUNCTION(_prefix)
#else
#include <nuttx/config.h>
#define PX4_MAIN_FUNCTION(_prefix) __EXPORT int _prefix##main(int argc, char **argv)() { return main(argc, argv); }
#endif

95
src/platforms/px4_middleware.h Normal file
View File

@ -0,0 +1,95 @@
/****************************************************************************
*
* 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_middleware.h
*
* PX4 generic middleware wrapper
*/
#pragma once
#include <stdint.h>
namespace px4
{
void init(int argc, char *argv[], const char *process_name);
uint64_t get_time_micros();
bool ok();
void spin_once();
void spin();
class Rate
{
public:
explicit Rate(unsigned rate_hz) { sleep_interval = 1e6 / rate_hz; }
void sleep() { usleep(sleep_interval); }
private:
uint64_t sleep_interval;
};
// /**
// * A limiter/ saturation.
// * The output of update is the input, bounded
// * by min/max.
// */
// class __EXPORT BlockLimit : public Block
// {
// public:
// // methods
// BlockLimit(SuperBlock *parent, const char *name) :
// Block(parent, name),
// _min(this, "MIN"),
// _max(this, "MAX")
// {};
// virtual ~BlockLimit() {};
// float update(float input);
// // accessors
// float getMin() { return _min.get(); }
// float getMax() { return _max.get(); }
// protected:
// // attributes
// control::BlockParamFloat _min;
// control::BlockParamFloat _max;
// };
} // namespace px4

46
src/platforms/px4_nodehandle.h Normal file
View File

@ -0,0 +1,46 @@
/****************************************************************************
*
* 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_nodehandle.h
*
* PX4 Middleware Wrapper Node Handle
*/
namespace px4
{
class NodeHandle
{
};
}

46
src/platforms/px4_publisher.h Normal file
View File

@ -0,0 +1,46 @@
/****************************************************************************
*
* 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_nodehandle.h
*
* PX4 Middleware Wrapper Node Handle
*/
namespace px4
{
class Publisher
{
};
}

46
src/platforms/px4_subscriber.h Normal file
View File

@ -0,0 +1,46 @@
/****************************************************************************
*
* 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_subscriber.h
*
* PX4 Middleware Wrapper Subscriber
*/
namespace px4
{
class Subscriber
{
};
}

40
src/platforms/ros/px4_publisher.cpp Normal file
View File

@ -0,0 +1,40 @@
/****************************************************************************
*
* 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_publisher.cpp
*
* PX4 Middleware Wrapper for Publisher
*/

71
src/platforms/ros/px4_ros_impl.cpp Normal file
View File

@ -0,0 +1,71 @@
/****************************************************************************
*
* 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_ros_impl.cpp
*
* PX4 Middleware Wrapper ROS Implementation
*/
#include <px4.h>
namespace px4
{
void init(int argc, char *argv[], const char *process_name)
{
ros::init(argc, argv, process_name);
}
uint64_t get_time_micros()
{
ros::Time time = ros::Time::now();
return time.sec * 1e6 + time.nsec / 1000;
}
bool ok()
{
return ros::ok();
}
void spin_once()
{
ros::spinOnce();
}
void spin()
{
ros::spin();
}
}

40
src/platforms/ros/px4_subscriber.cpp Normal file
View File

@ -0,0 +1,40 @@
/****************************************************************************
*
* 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_subscriber.cpp
*
* PX4 Middleware Wrapper Subscriber
*/