From 2bbc98067a16621b49f103fc3e1ae099ab957aa8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 10 Nov 2014 07:31:55 +0100 Subject: Complementing ROS wrapper, not compiling yet --- src/examples/publisher/publisher.cpp | 4 +- src/examples/subscriber/subscriber.cpp | 6 +-- src/platforms/nuttx/px4_nodehandle.cpp | 58 +++++++++++++++++++++++++++ src/platforms/nuttx/px4_nuttx_impl.cpp | 19 --------- src/platforms/px4_middleware.h | 8 +--- src/platforms/px4_nodehandle.h | 24 +++++++++++- src/platforms/px4_publisher.h | 2 + src/platforms/px4_subscriber.h | 2 + src/platforms/ros/px4_nodehandle.cpp | 71 ++++++++++++++++++++++++++++++++++ src/platforms/ros/px4_ros_impl.cpp | 15 ------- 10 files changed, 163 insertions(+), 46 deletions(-) create mode 100644 src/platforms/nuttx/px4_nodehandle.cpp create mode 100644 src/platforms/ros/px4_nodehandle.cpp (limited to 'src') diff --git a/src/examples/publisher/publisher.cpp b/src/examples/publisher/publisher.cpp index 91e063162..7e2b2f0d4 100644 --- a/src/examples/publisher/publisher.cpp +++ b/src/examples/publisher/publisher.cpp @@ -67,7 +67,7 @@ int main(int argc, char **argv) */ int count = 0; - while (px4::ok()) { + while (n.ok()) { /** * This is a message object. You stuff it with data, and then publish it. */ @@ -84,7 +84,7 @@ int main(int argc, char **argv) */ rc_channels_pub.publish(msg); - px4::spin_once(); + n.spinOnce(); loop_rate.sleep(); ++count; diff --git a/src/examples/subscriber/subscriber.cpp b/src/examples/subscriber/subscriber.cpp index bf16bf84e..626ceeb25 100644 --- a/src/examples/subscriber/subscriber.cpp +++ b/src/examples/subscriber/subscriber.cpp @@ -57,7 +57,7 @@ int main(int argc, char **argv) * The first NodeHandle constructed will fully initialize this node, and the last * NodeHandle destructed will close down the node. */ - ros::NodeHandle n; + px4::NodeHandle n; /** * The subscribe() call is how you tell ROS that you want to receive messages @@ -74,14 +74,14 @@ int main(int argc, char **argv) * 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); + px4::Subscriber sub = n.subscribe("rc_channels", 1000, rc_channels_callback); /** * 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(); + n.spin(); return 0; } diff --git a/src/platforms/nuttx/px4_nodehandle.cpp b/src/platforms/nuttx/px4_nodehandle.cpp new file mode 100644 index 000000000..bdc659c40 --- /dev/null +++ b/src/platforms/nuttx/px4_nodehandle.cpp @@ -0,0 +1,58 @@ +/**************************************************************************** + * + * 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.cpp + * + * PX4 Middleware Wrapper Node Handle + */ + +namespace px4 { + +void NodeHandle::spinOnce() +{ + // XXX implement orb_check(); +} + +void NodeHandle::spin() +{ + // XXX polling wait +} + +bool NodeHandle::ok() +{ + return !task_should_exit; +} + +} + diff --git a/src/platforms/nuttx/px4_nuttx_impl.cpp b/src/platforms/nuttx/px4_nuttx_impl.cpp index 3a6529716..c644dbced 100644 --- a/src/platforms/nuttx/px4_nuttx_impl.cpp +++ b/src/platforms/nuttx/px4_nuttx_impl.cpp @@ -39,8 +39,6 @@ #include -extern bool task_should_exit; - namespace px4 { @@ -55,21 +53,4 @@ 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 - -} - } diff --git a/src/platforms/px4_middleware.h b/src/platforms/px4_middleware.h index d1c0656af..d7bb1fbae 100644 --- a/src/platforms/px4_middleware.h +++ b/src/platforms/px4_middleware.h @@ -41,6 +41,8 @@ #include +#include "px4_nodehandle.h" + namespace px4 { @@ -48,12 +50,6 @@ void init(int argc, char *argv[], const char *process_name); uint64_t get_time_micros(); -bool ok(); - -void spin_once(); - -void spin(); - class Rate { diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h index d278828b7..50424c215 100644 --- a/src/platforms/px4_nodehandle.h +++ b/src/platforms/px4_nodehandle.h @@ -37,10 +37,32 @@ * PX4 Middleware Wrapper Node Handle */ +#pragma once + +#include "px4_publisher.h" +#include "px4_subscriber.h" + namespace px4 { -class NodeHandle + +template + class NodeHandle { +public: + void spin_once(); + + void spin(); + + template Publisher + advertise (const char* topic, uint32_t queue_size, bool latch=false); + + template Subscriber + subscribe(const char* topic_name, uint32_t queue_size, + const std::function< void(const std::shared_ptr< M const > &)> &callback); + +protected: + bool task_should_exit; }; + } diff --git a/src/platforms/px4_publisher.h b/src/platforms/px4_publisher.h index 1b0952155..1a77fa3df 100644 --- a/src/platforms/px4_publisher.h +++ b/src/platforms/px4_publisher.h @@ -37,6 +37,8 @@ * PX4 Middleware Wrapper Node Handle */ +#pragma once + namespace px4 { class Publisher diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h index 8759f8b05..30c5eee58 100644 --- a/src/platforms/px4_subscriber.h +++ b/src/platforms/px4_subscriber.h @@ -37,6 +37,8 @@ * PX4 Middleware Wrapper Subscriber */ +#pragma once + namespace px4 { class Subscriber diff --git a/src/platforms/ros/px4_nodehandle.cpp b/src/platforms/ros/px4_nodehandle.cpp new file mode 100644 index 000000000..274a447c8 --- /dev/null +++ b/src/platforms/ros/px4_nodehandle.cpp @@ -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_nodehandle.cpp + * + * PX4 Middleware Wrapper Node Handle + */ + +namespace px4 { + +void NodeHandle::spinOnce() +{ + ros::spinOnce(); +} + +void NodeHandle::spin() +{ + ros::spin(); +} + +bool NodeHandle::ok() +{ + return ros::ok(); +} + +template Publisher +NodeHandle::advertise (const char* topic, uint32_t queue_size, bool latch=false) +{ + +} + +template Subscriber +NodeHandle::subscribe(const char* topic_name, uint32_t queue_size, + const std::function< void(const std::shared_ptr< M const > &)> &callback) +{ + +} + +} + diff --git a/src/platforms/ros/px4_ros_impl.cpp b/src/platforms/ros/px4_ros_impl.cpp index eda17e5a9..854986a7f 100644 --- a/src/platforms/ros/px4_ros_impl.cpp +++ b/src/platforms/ros/px4_ros_impl.cpp @@ -53,19 +53,4 @@ uint64_t get_time_micros() return time.sec * 1e6 + time.nsec / 1000; } -bool ok() -{ - return ros::ok(); -} - -void spin_once() -{ - ros::spinOnce(); -} - -void spin() -{ - ros::spin(); -} - } -- cgit v1.2.3