diff --git a/Tools/HIL/run_tests.py b/Tools/HIL/run_tests.py index 3e79c70227..0d9c5f5d1b 100755 --- a/Tools/HIL/run_tests.py +++ b/Tools/HIL/run_tests.py @@ -136,9 +136,6 @@ class TestHardwareMethods(unittest.TestCase): def test_atomic_bitset(self): self.assertTrue(do_test(self.TEST_DEVICE, self.TEST_BAUDRATE, "atomic_bitset")) - def test_bezier(self): - self.assertTrue(do_test(self.TEST_DEVICE, self.TEST_BAUDRATE, "bezier")) - def test_bitset(self): self.assertTrue(do_test(self.TEST_DEVICE, self.TEST_BAUDRATE, "bitset")) diff --git a/src/systemcmds/tests/CMakeLists.txt b/src/systemcmds/tests/CMakeLists.txt index 72d99c6708..2beab87943 100644 --- a/src/systemcmds/tests/CMakeLists.txt +++ b/src/systemcmds/tests/CMakeLists.txt @@ -33,7 +33,6 @@ set(srcs test_atomic_bitset.cpp - test_bezierQuad.cpp test_bitset.cpp test_bson.cpp test_dataman.cpp diff --git a/src/systemcmds/tests/test_bezierQuad.cpp b/src/systemcmds/tests/test_bezierQuad.cpp deleted file mode 100644 index 9fc5f7f987..0000000000 --- a/src/systemcmds/tests/test_bezierQuad.cpp +++ /dev/null @@ -1,278 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2018-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. - * - ****************************************************************************/ - -/** - * @file test_bezierQuad.cpp - * Test for Bezier curve computation. - */ - -#include -#include -#include -#include - -#include "../../lib/bezier/BezierQuad.hpp" - -class BezierQuadTest : public UnitTest -{ -public: - virtual bool run_tests(); - -private: - - bool _get_states_from_time(); - bool _get_arc_length(); - bool _set_bez_from_vel(); - - float random(float min, float max); - -}; - - -bool BezierQuadTest::run_tests() -{ - ut_run_test(_get_states_from_time); - ut_run_test(_get_arc_length); - ut_run_test(_set_bez_from_vel); - - return (_tests_failed == 0); -} - -bool BezierQuadTest::_get_states_from_time() -{ - // symmetric around 0 - matrix::Vector3f pt0(-0.5f, 0.0f, 0.0f); - matrix::Vector3f ctrl(0.0f, 0.5f, 0.0f); - matrix::Vector3f pt1(0.5f, 0.0f, 0.0f); - - // create bezier with default t = [0,1] - bezier::BezierQuad_f bz(pt0, ctrl, pt1); - - matrix::Vector3f pos, vel, acc; - float precision = 0.00001; - - // states at time = 0 - bz.getStates(pos, vel, acc, 0.0f); - - ut_compare_float("pos[0] not equal pt0[0]", pos(0), pt0(0), precision); - ut_compare_float("pos[1] not equal pt0[1]", pos(1), pt0(1), precision); - ut_compare_float("pos[2] not equal pt0[2]", pos(2), pt0(2), precision); - - ut_compare_float("slope not equal 1", vel(0), 1.0f, precision); - ut_compare_float("slope not equal 1", vel(1), 1.0f, precision); - ut_compare_float("slope not equal 0", vel(2), 0.0f, precision); - - ut_compare_float("acc not equal 0", acc(0), 0.0f, precision); - ut_compare_float("acc not equal 1", acc(1), -2.0f, precision); - ut_compare_float("acc not equal 0", acc(2), 0.0f, precision); - - // states at time = 1 - bz.getStates(pos, vel, acc, 1.0f); - - ut_compare_float("pos[0] not equal pt1[0]", pos(0), pt1(0), precision); - ut_compare_float("pos[1] not equal pt1[1]", pos(1), pt1(1), precision); - ut_compare_float("pos[2] not equal pt1[2]", pos(2), pt1(2), precision); - - ut_compare_float("slope not equal 1", vel(0), 1.0f, precision); - ut_compare_float("slope not equal -1", vel(1), -1.0f, precision); - ut_compare_float("slope not equal 0", vel(2), 0.0f, precision); - - ut_compare_float("acc not equal 0", acc(0), 0.0f, precision); - ut_compare_float("acc not equal 1", acc(1), -2.0f, precision); - ut_compare_float("acc not equal 0", acc(2), 0.0f, precision); - - // states at time = 0.5 - bz.getStates(pos, vel, acc, 0.50f); - - // pos must be equal to ctrl(0) and lower than ctrl(1) - ut_compare_float("pos[0] not equal ctrl[0]", pos(0), ctrl(0), precision); - ut_assert_true(pos(1) < ctrl(1)); - - ut_compare_float("slope not equal 1", vel(0), 1.0f, precision); - ut_compare_float("slope not equal -1", vel(1), 0.0f, precision); - ut_compare_float("slope not equal 0", vel(2), 0.0f, precision); - - ut_compare_float("acc not equal 0", acc(0), 0.0f, precision); - ut_compare_float("acc not equal -2", acc(1), -2.0f, precision); - ut_compare_float("acc not equal 0", acc(2), 0.0f, precision); - - // acceleration - pt0 = matrix::Vector3f(0.0f, 0.0f, 0.0f); - ctrl = matrix::Vector3f(0.0f, 0.0f, 0.0f); - pt1 = matrix::Vector3f(1.0f, 0.0f, 0.0f); - - // create bezier with default t = [0,1] - bz.setBezier(pt0, ctrl, pt1, 1.0f); - - // states at time = 0.0 - bz.getStates(pos, vel, acc, 0.0f); - - ut_compare_float("pos[0] not equal pt0[0]", pos(0), pt0(0), precision); - ut_compare_float("pos[1] not equal pt0[1]", pos(1), pt0(1), precision); - ut_compare_float("pos[2] not equal pt0[2]", pos(2), pt0(2), precision); - - ut_compare_float("slope not equal 0", vel(0), 0.0f, precision); - ut_compare_float("slope not equal 0", vel(1), 0.0f, precision); - ut_compare_float("slope not equal 0", vel(2), 0.0f, precision); - - ut_compare_float("acc not equal 2", acc(0), 2.0f, precision); - ut_compare_float("acc not equal 0", acc(1), 0.0f, precision); - ut_compare_float("acc not equal 0", acc(2), 0.0f, precision); - - // states at time = 1.0 - bz.getStates(pos, vel, acc, 1.0f); - - ut_compare_float("pos[0] not equal pt1[0]", pos(0), pt1(0), precision); - ut_compare_float("pos[1] not equal pt1[1]", pos(1), pt1(1), precision); - ut_compare_float("pos[2] not equal pt1[2]", pos(2), pt1(2), precision); - - ut_compare_float("slope not equal 2", vel(0), 2.0f, precision); - ut_compare_float("slope not equal 0", vel(1), 0.0f, precision); - ut_compare_float("slope not equal 0", vel(2), 0.0f, precision); - - ut_compare_float("acc not equal 2", acc(0), 2.0f, precision); - ut_compare_float("acc not equal 0", acc(1), 0.0f, precision); - ut_compare_float("acc not equal 0", acc(2), 0.0f, precision); - - // states at time = 0.5 - bz.getStates(pos, vel, acc, 0.5f); - - ut_compare_float("slope not equal 1", vel(0), 1.0f, precision); - ut_compare_float("slope not equal 0", vel(1), 0.0f, precision); - ut_compare_float("slope not equal 0", vel(2), 0.0f, precision); - - ut_compare_float("acc not equal 2", acc(0), 2.0f, precision); - ut_compare_float("acc not equal 0", acc(1), 0.0f, precision); - ut_compare_float("acc not equal 0", acc(2), 0.0f, precision); - - return true; - -} - -bool BezierQuadTest::_get_arc_length() -{ - // create random numbers - srand(0); // choose a constant to make it deterministic - - float min = -50.f; - float max = 50.f; - float resolution = 0.1f; - - matrix::Vector3f pt0, pt1, ctrl; - float duration, arc_length, triangle_length, straigth_length; - float T = 100.0f; - - // loop trough different control points 100x and check if arc_length is in the expected range - for (int i = 0; i < 100 ; i++) { - // random bezier point - pt0 = matrix::Vector3f(random(min, max), random(min, max), random(min, max)); - pt1 = matrix::Vector3f(random(min, max), random(min, max), random(min, max)); - ctrl = matrix::Vector3f(random(min, max), random(min, max), random(min, max)); - - // use for each test a new duration - duration = random(0.0f, T); - - // create bezier - bezier::BezierQuad_f bz(pt0, ctrl, pt1, duration); - - // compute arc length, triangle length and straigh length - arc_length = bz.getArcLength(resolution); - triangle_length = (ctrl - pt0).length() + (pt1 - ctrl).length(); - straigth_length = (pt1 - pt0).length(); - - // we also compute length from going point to point and add segment - float time_increment = duration / T; - float t = 0.0f + time_increment; - matrix::Vector3f p0 = pt0; - float sum_segments = 0.0f; - - for (int s = 0; s < (int)T; s++) { - matrix::Vector3f nextpt = bz.getPoint(t); - sum_segments = (nextpt - p0).length() + sum_segments; - p0 = bz.getPoint(t); - t = t + time_increment; - } - - // test comparisons - ut_assert_true((triangle_length >= arc_length) && (arc_length >= straigth_length) - && (fabsf(arc_length - sum_segments) < 1.f)); - } - - - return true; -} - -bool BezierQuadTest::_set_bez_from_vel() -{ - // create random numbers - srand(100); // choose a constant to make it deterministic - - float low = -50.0f; - float max = 50.0f; - float precision = 0.001f; - - for (int i = 0; i < 20; i++) { - - // set velocity - matrix::Vector3f ctrl(random(low, max), random(low, max), random(low, max)); - matrix::Vector3f vel0(random(low, max), random(low, max), random(low, max)); - matrix::Vector3f vel1(random(low, max), random(low, max), random(low, max)); - float duration = random(0.0f, 100.0f); - - bezier::BezierQuad_f bz;; - bz.setBezFromVel(ctrl, vel0, vel1, duration); - - // get velocity back - matrix::Vector3f v0 = bz.getVelocity(0.0f); - matrix::Vector3f v1 = bz.getVelocity(duration); - ut_compare_float("", vel0(0), v0(0), precision); - ut_compare_float("", vel1(0), v1(0), precision); - - ut_compare_float("", vel0(1), v0(1), precision); - ut_compare_float("", vel1(1), v1(1), precision); - - ut_compare_float("", vel0(2), v0(2), precision); - ut_compare_float("", vel1(2), v1(2), precision); - } - - return true; -} - -float BezierQuadTest::random(float min, float max) -{ - float s = rand() / (float)RAND_MAX; - return (min + s * (max - min)); - -} - -ut_declare_test_c(test_bezierQuad, BezierQuadTest) diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c index af25fd074e..3d04ad31ef 100644 --- a/src/systemcmds/tests/tests_main.c +++ b/src/systemcmds/tests/tests_main.c @@ -78,7 +78,6 @@ const struct { #endif /* __PX4_NUTTX */ {"atomic_bitset", test_atomic_bitset, 0}, - {"bezier", test_bezierQuad, 0}, {"bitset", test_bitset, 0}, {"bson", test_bson, 0}, {"dataman", test_dataman, OPT_NOJIGTEST | OPT_NOALLTEST}, diff --git a/src/systemcmds/tests/tests_main.h b/src/systemcmds/tests/tests_main.h index 0e5a14743c..c42bce07fc 100644 --- a/src/systemcmds/tests/tests_main.h +++ b/src/systemcmds/tests/tests_main.h @@ -44,7 +44,6 @@ __BEGIN_DECLS extern int test_atomic_bitset(int argc, char *argv[]); -extern int test_bezierQuad(int argc, char *argv[]); extern int test_bitset(int argc, char *argv[]); extern int test_bson(int argc, char *argv[]); extern int test_dataman(int argc, char *argv[]);