From edad4c40c3f97ffbe2f3d7edda0a09e3e5136a40 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Fri, 22 Feb 2019 13:34:23 -0500
Subject: [PATCH] containers add IntrusiveQueue and testing
---
platforms/posix/cmake/sitl_tests.cmake | 1 +
src/include/containers/IntrusiveQueue.hpp | 115 ++++++++++
src/systemcmds/tests/CMakeLists.txt | 1 +
src/systemcmds/tests/test_IntrusiveQueue.cpp | 214 +++++++++++++++++++
src/systemcmds/tests/tests_main.c | 1 +
src/systemcmds/tests/tests_main.h | 1 +
6 files changed, 333 insertions(+)
create mode 100644 src/include/containers/IntrusiveQueue.hpp
create mode 100644 src/systemcmds/tests/test_IntrusiveQueue.cpp
diff --git a/platforms/posix/cmake/sitl_tests.cmake b/platforms/posix/cmake/sitl_tests.cmake
index 1b329e6e05..fa537217bd 100644
--- a/platforms/posix/cmake/sitl_tests.cmake
+++ b/platforms/posix/cmake/sitl_tests.cmake
@@ -17,6 +17,7 @@ set(tests
hrt
hysteresis
int
+ IntrusiveQueue
List
mathlib
matrix
diff --git a/src/include/containers/IntrusiveQueue.hpp b/src/include/containers/IntrusiveQueue.hpp
new file mode 100644
index 0000000000..100017cd29
--- /dev/null
+++ b/src/include/containers/IntrusiveQueue.hpp
@@ -0,0 +1,115 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2019 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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <stdlib.h>
+
+template<class T>
+class IntrusiveQueue
+{
+public:
+
+ bool empty() const { return _head == nullptr; }
+
+ T front() const { return _head; }
+ T back() const { return _tail; }
+
+ size_t size() const
+ {
+ size_t sz = 0;
+
+ for (auto node = front(); node != nullptr; node = node->next_intrusive_queue_node()) {
+ sz++;
+ }
+
+ return sz;
+ }
+
+ void push(T newNode)
+ {
+ // error, node already queued or already inserted
+ if ((newNode->next_intrusive_queue_node() != nullptr) || (newNode == _tail)) {
+ return;
+ }
+
+ if (_head == nullptr) {
+ _head = newNode;
+ }
+
+ if (_tail != nullptr) {
+ _tail->set_next_intrusive_queue_node(newNode);
+ }
+
+ _tail = newNode;
+ }
+
+ T pop()
+ {
+ T ret = _head;
+
+ if (!empty()) {
+ if (_head != _tail) {
+ _head = _head->next_intrusive_queue_node();
+
+ } else {
+ // only one item left
+ _head = nullptr;
+ _tail = nullptr;
+ }
+
+ // clear next in popped (in might be re-inserted later)
+ ret->set_next_intrusive_queue_node(nullptr);
+ }
+
+ return ret;
+ }
+
+private:
+
+ T _head{nullptr};
+ T _tail{nullptr};
+
+};
+
+template<class T>
+class IntrusiveQueueNode
+{
+private:
+ friend IntrusiveQueue<T>;
+
+ T next_intrusive_queue_node() const { return _next_intrusive_queue_node; }
+ void set_next_intrusive_queue_node(T new_next) { _next_intrusive_queue_node = new_next; }
+
+ T _next_intrusive_queue_node{nullptr};
+};
diff --git a/src/systemcmds/tests/CMakeLists.txt b/src/systemcmds/tests/CMakeLists.txt
index d62920cb75..01d1eccaef 100644
--- a/src/systemcmds/tests/CMakeLists.txt
+++ b/src/systemcmds/tests/CMakeLists.txt
@@ -46,6 +46,7 @@ set(srcs
test_hrt.cpp
test_hysteresis.cpp
test_int.cpp
+ test_IntrusiveQueue.cpp
test_jig_voltages.c
test_led.c
test_List.cpp
diff --git a/src/systemcmds/tests/test_IntrusiveQueue.cpp b/src/systemcmds/tests/test_IntrusiveQueue.cpp
new file mode 100644
index 0000000000..d82f7607f4
--- /dev/null
+++ b/src/systemcmds/tests/test_IntrusiveQueue.cpp
@@ -0,0 +1,214 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2019 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.
+ *
+ ****************************************************************************/
+
+#include <unit_test.h>
+#include <containers/IntrusiveQueue.hpp>
+#include <float.h>
+#include <math.h>
+
+class testContainer : public IntrusiveQueueNode<testContainer *>
+{
+public:
+ int i{0};
+};
+
+class IntrusiveQueueTest : public UnitTest
+{
+public:
+ virtual bool run_tests();
+
+ bool test_push();
+ bool test_pop();
+ bool test_push_duplicate();
+
+};
+
+bool IntrusiveQueueTest::run_tests()
+{
+ ut_run_test(test_push);
+ ut_run_test(test_pop);
+ ut_run_test(test_push_duplicate);
+
+ return (_tests_failed == 0);
+}
+
+bool IntrusiveQueueTest::test_push()
+{
+ IntrusiveQueue<testContainer *> q1;
+
+ // size should be 0 initially
+ ut_compare("size initially 0", q1.size(), 0);
+ ut_assert_true(q1.empty());
+
+ // insert 100
+ for (int i = 0; i < 100; i++) {
+ testContainer *t = new testContainer();
+ t->i = i;
+
+ ut_compare("size increasing with i", q1.size(), i);
+ q1.push(t);
+ ut_compare("size increasing with i", q1.size(), i + 1);
+
+ ut_assert_true(!q1.empty());
+ }
+
+ // verify full size (100)
+ ut_compare("size 100", q1.size(), 100);
+
+ // pop all elements
+ for (int i = 0; i < 100; i++) {
+ auto node = q1.front();
+ q1.pop();
+ delete node;
+ }
+
+ // verify list has been cleared
+ ut_compare("size 0", q1.size(), 0);
+ ut_assert_true(q1.empty());
+
+ return true;
+}
+
+bool IntrusiveQueueTest::test_pop()
+{
+ IntrusiveQueue<testContainer *> q1;
+
+ // size should be 0 initially
+ ut_compare("size initially 0", q1.size(), 0);
+ ut_assert_true(q1.empty());
+
+ // insert 100
+ for (int i = 0; i < 100; i++) {
+ testContainer *t = new testContainer();
+ t->i = i;
+ q1.push(t);
+ }
+
+ // verify full size (100)
+ ut_assert_true(q1.size() == 100);
+
+ for (int i = 0; i < 100; i++) {
+ auto node = q1.front();
+ ut_compare("stored i", i, node->i);
+
+ ut_compare("size check", q1.size(), 100 - i);
+ q1.pop();
+ ut_compare("size check", q1.size(), 100 - i - 1);
+
+ delete node;
+
+ ut_compare("size check", q1.size(), 100 - i - 1);
+ }
+
+ // verify list has been cleared
+ ut_assert_true(q1.empty());
+ ut_compare("size check", q1.size(), 0);
+
+ // pop an empty queue
+ auto T = q1.pop();
+ ut_assert_true(T == nullptr);
+ ut_assert_true(q1.empty());
+ ut_compare("size check", q1.size(), 0);
+
+ return true;
+}
+
+bool IntrusiveQueueTest::test_push_duplicate()
+{
+ IntrusiveQueue<testContainer *> q1;
+
+ // size should be 0 initially
+ ut_compare("size initially 0", q1.size(), 0);
+ ut_assert_true(q1.empty());
+
+ // insert 100
+ for (int i = 0; i < 100; i++) {
+ testContainer *t = new testContainer();
+ t->i = i;
+
+ ut_compare("size increasing with i", q1.size(), i);
+ q1.push(t);
+ ut_compare("size increasing with i", q1.size(), i + 1);
+
+ ut_assert_true(!q1.empty());
+ }
+
+ // verify full size (100)
+ ut_compare("size 100", q1.size(), 100);
+
+
+ // attempt to insert front again
+ const auto q1_front = q1.front();
+ const auto q1_front_i = q1_front->i; // copy i value
+
+ const auto q1_back = q1.back();
+ const auto q1_back_i = q1_back->i; // copy i value
+
+ // push front and back aagain
+ q1.push(q1_front);
+ q1.push(q1_back);
+
+ // verify no change
+ ut_compare("size 100", q1.size(), 100);
+ ut_compare("q front not reinserted", q1.front()->i, q1_front->i);
+ ut_compare("q back not reinserted", q1.back()->i, q1_back->i);
+
+
+ // pop the head
+ const auto q1_head = q1.pop();
+
+ // verfify size should now be 99
+ ut_compare("size 99", q1.size(), 99);
+
+ // push back on
+ q1.push(q1_head);
+
+ // verify size now 100 again
+ ut_compare("size 100", q1.size(), 100);
+
+
+ // pop all elements
+ for (int i = 0; i < 100; i++) {
+ auto node = q1.front();
+ q1.pop();
+ delete node;
+ }
+
+ // verify list has been cleared
+ ut_compare("size 0", q1.size(), 0);
+ ut_assert_true(q1.empty());
+
+ return true;
+}
+
+ut_declare_test_c(test_IntrusiveQueue, IntrusiveQueueTest)
diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c
index b1dfd86c80..14cf01836b 100644
--- a/src/systemcmds/tests/tests_main.c
+++ b/src/systemcmds/tests/tests_main.c
@@ -95,6 +95,7 @@ const struct {
{"hrt", test_hrt, OPT_NOJIGTEST | OPT_NOALLTEST},
{"hysteresis", test_hysteresis, 0},
{"int", test_int, 0},
+ {"IntrusiveQueue", test_IntrusiveQueue, 0},
{"jig_voltages", test_jig_voltages, OPT_NOALLTEST},
{"List", test_List, 0},
{"mathlib", test_mathlib, 0},
diff --git a/src/systemcmds/tests/tests_main.h b/src/systemcmds/tests/tests_main.h
index 5d9d0fc3e8..40743cc9e4 100644
--- a/src/systemcmds/tests/tests_main.h
+++ b/src/systemcmds/tests/tests_main.h
@@ -57,6 +57,7 @@ extern int test_hott_telemetry(int argc, char *argv[]);
extern int test_hrt(int argc, char *argv[]);
extern int test_hysteresis(int argc, char *argv[]);
extern int test_int(int argc, char *argv[]);
+extern int test_IntrusiveQueue(int argc, char *argv[]);
extern int test_jig_voltages(int argc, char *argv[]);
extern int test_led(int argc, char *argv[]);
extern int test_List(int argc, char *argv[]);
--
GitLab