From 5b8aa20c2f2bbeb7e23ca6889558bbdc1cea122e Mon Sep 17 00:00:00 2001 From: Julian Kent Date: Tue, 18 Feb 2020 16:13:27 +0100 Subject: [PATCH] Bezier trajectory functions to get position, velocity, accel on trajectory --- src/lib/bezier/BezierN.cpp | 163 +++++++++++++++++ src/lib/bezier/BezierN.hpp | 76 ++++++++ src/lib/bezier/BezierNTest.cpp | 325 +++++++++++++++++++++++++++++++++ src/lib/bezier/BezierQuad.cpp | 8 + src/lib/bezier/CMakeLists.txt | 3 + 5 files changed, 575 insertions(+) create mode 100644 src/lib/bezier/BezierN.cpp create mode 100644 src/lib/bezier/BezierN.hpp create mode 100644 src/lib/bezier/BezierNTest.cpp diff --git a/src/lib/bezier/BezierN.cpp b/src/lib/bezier/BezierN.cpp new file mode 100644 index 0000000000..0d30aebd88 --- /dev/null +++ b/src/lib/bezier/BezierN.cpp @@ -0,0 +1,163 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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 BezierN.cpp + * Bezier function + * + * @author Julian Kent + */ + +#include +#include + +namespace +{ + +/* + * Generic in-place bezier implementation. Leaves result in first element. + * + */ +template +void calculateBezier(matrix::Vector *positions, int N, Scalar t, Scalar one_minus_t) +{ + for (int bezier_order = 1; bezier_order < N; bezier_order++) { + for (int i = 0; i < N - bezier_order; i++) { + positions[i] = positions[i] * one_minus_t + positions[i + 1] * t; + } + } +} +} + +namespace bezier +{ + +bool calculateBezierPosVel(const matrix::Vector3f *positions, int N, float t, + matrix::Vector3f &position, matrix::Vector3f &velocity) +{ + if (positions == nullptr || N == 0 || t < 0 || t > 1) { + return false; + } + + using Df = matrix::Dual; + using Vector3Df = matrix::Vector3; + + Vector3Df intermediates[N]; + + for (int i = 0; i < N; i++) { + for (int j = 0; j < 3; j++) { + intermediates[i](j) = positions[i](j); + } + } + + Df dual_t(t, 0); // derivative with respect to time + calculateBezier(intermediates, N, dual_t, Df(1) - dual_t); + + position = matrix::collectReals(intermediates[0]); + velocity = matrix::collectDerivatives(intermediates[0]); + + return true; +} + +bool calculateBezierPosVelAcc(const matrix::Vector3f *positions, int N, float t, + matrix::Vector3f &position, matrix::Vector3f &velocity, matrix::Vector3f &acceleration) +{ + if (positions == nullptr || N == 0 || t < 0 || t > 1) { + return false; + } + + using Df = matrix::Dual; + using DDf = matrix::Dual; + using Vector3DDf = matrix::Vector3; + + Vector3DDf intermediates[N]; + + for (int i = 0; i < N; i++) { + for (int j = 0; j < 3; j++) { + intermediates[i](j) = Df(positions[i](j)); + } + } + + DDf dual_t(Df(t, 0), 0); // 1st and 2nd derivative with respect to time + calculateBezier(intermediates, N, dual_t, Df(1) - dual_t); + + position = matrix::collectReals(matrix::collectReals(intermediates[0])); + velocity = matrix::collectReals(matrix::collectDerivatives(intermediates[0])); + acceleration = matrix::collectDerivatives(matrix::collectDerivatives(intermediates[0])); + + return true; +} + +bool calculateBezierYaw(const float *setpoints, int N, float t, float &yaw_setpoint, float &yaw_vel_setpoint) +{ + if (setpoints == nullptr || N == 0 || t < 0 || t > 1) { + return false; + } + + + using Df = matrix::Dual; + using Vector1Df = matrix::Vector; + + Vector1Df intermediates[N]; + + // all yaw setpoints are wrapped relative to the starting yaw + const float offset = setpoints[0]; + + for (int i = 0; i < N; i++) { + intermediates[i](0) = matrix::wrap_pi(setpoints[i] - offset); + } + + Df dual_t (t, 0); // derivative with respect to time + calculateBezier(intermediates, N, dual_t, Df(1) - dual_t); + + Df result = intermediates[0](0); + yaw_setpoint = matrix::wrap_pi(result.value + offset); + yaw_vel_setpoint = result.derivative(0); + + return true; +} + +bool calculateT(int64_t start_time, int64_t end_time, int64_t now, float &T) +{ + if (now < start_time || end_time < now) { + return false; + } + + int64_t total_duration = end_time - start_time; + int64_t elapsed_duration = now - start_time; + + T = (float) elapsed_duration / (float) total_duration; + + return true; +} + +} diff --git a/src/lib/bezier/BezierN.hpp b/src/lib/bezier/BezierN.hpp new file mode 100644 index 0000000000..32e368c14e --- /dev/null +++ b/src/lib/bezier/BezierN.hpp @@ -0,0 +1,76 @@ + + +/**************************************************************************** + * + * Copyright (C) 2020 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 BerzierN.hpp + * + * @author Julian Kent + * + * N-order Bezier library designed for time-aware trajectory tracking + */ + +#pragma once +#include + +namespace bezier +{ + +/* + * Calculates the location and velocity with respect to T on a given bezier curve of any order. + * + */ +bool calculateBezierPosVel(const matrix::Vector3f *positions, int N, float t, + matrix::Vector3f &position, matrix::Vector3f &velocity); + +/* + * Calculates the position, velocity and acceleration with respect to T on a given bezier curve of any order. + * + */ +bool calculateBezierPosVelAcc(const matrix::Vector3f *positions, int N, float t, + matrix::Vector3f &position, matrix::Vector3f &velocity, matrix::Vector3f &acceleration); + +/* + * Calculates the position and velocity of yaw with respect to t on a bezier curve. + * All yaw setpoints are wrapped relative to the starting yaw. + * + */ +bool calculateBezierYaw(const float *setpoints, int N, float t, float &yaw_setpoint, float &yaw_vel_setpoint); + +/* + * Calculates the fraction between the begin and end time which can be used for fast bezier curve lookups + */ +bool calculateT(int64_t start_time, int64_t end_time, int64_t now, float &T); + +} diff --git a/src/lib/bezier/BezierNTest.cpp b/src/lib/bezier/BezierNTest.cpp new file mode 100644 index 0000000000..b08a40fd62 --- /dev/null +++ b/src/lib/bezier/BezierNTest.cpp @@ -0,0 +1,325 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * Test code for the Velocity Smoothing library + * Run this test only using make tests TESTFILTER=BezierN + * + * @author Julian Kent + */ + +#include +#include + +#include "BezierN.hpp" + +TEST(BezierN_calculateBezier, checks_validity) +{ + matrix::Vector3f points[10]; + matrix::Vector3f a, b; + EXPECT_FALSE(bezier::calculateBezierPosVel(nullptr, 10, 0.5f, a, b)); + EXPECT_FALSE(bezier::calculateBezierPosVel(points, 0, 0.5f, a, b)); + EXPECT_FALSE(bezier::calculateBezierPosVel(points, 10, -0.5f, a, b)); + EXPECT_FALSE(bezier::calculateBezierPosVel(points, 10, 1.5f, a, b)); +} + +TEST(BezierN_calculateBezier, checks_validity_accel) +{ + matrix::Vector3f points[10]; + matrix::Vector3f a, b, c; + EXPECT_FALSE(bezier::calculateBezierPosVelAcc(nullptr, 10, 0.5f, a, b, c)); + EXPECT_FALSE(bezier::calculateBezierPosVelAcc(points, 0, 0.5f, a, b, c)); + EXPECT_FALSE(bezier::calculateBezierPosVelAcc(points, 10, -0.5f, a, b, c)); + EXPECT_FALSE(bezier::calculateBezierPosVelAcc(points, 10, 1.5f, a, b, c)); +} + +TEST(BezierN_calculateBezier, work_1_point) +{ + // GIVEN: a single point bezier curve + matrix::Vector3f points[2] = {matrix::Vector3f(1, 2, 3), matrix::Vector3f(NAN, NAN, NAN)}; + matrix::Vector3f pos, vel; + pos *= NAN; + vel *= NAN; + + // WHEN: we get the half-way point + EXPECT_TRUE(bezier::calculateBezierPosVel(points, 1, 0.5f, pos, vel)); + + // THEN: it should be the same as the point, and the velocity should be 0 + EXPECT_EQ((pos - points[0]).norm(), 0.f); + EXPECT_EQ(vel.norm(), 0.f); +} + +TEST(BezierN_calculateBezier, works_2_points) +{ + // GIVEN: a 2-point bezier curve + matrix::Vector3f points[3] = {matrix::Vector3f(1, 2, 3), matrix::Vector3f(5, 0, 1), matrix::Vector3f(NAN, NAN, NAN)}; + matrix::Vector3f pos, vel; + pos *= NAN; + vel *= NAN; + + // WHEN: we get the half-way point + EXPECT_TRUE(bezier::calculateBezierPosVel(points, 2, 0.5f, pos, vel)); + + // THEN: the position should be the mid-point between the start and end, and velocity should be the length + EXPECT_EQ((pos - matrix::Vector3f(3, 1, 2)).norm(), 0.f); + EXPECT_FLOAT_EQ((vel - (points[1] - points[0])).norm(), 0.f); + + // WHEN: we get the beginning point + EXPECT_TRUE(bezier::calculateBezierPosVel(points, 2, 0.f, pos, vel)); + + // THEN: the position should be the first point, and the velocity should still be the length + EXPECT_EQ((pos - points[0]).norm(), 0.f); + EXPECT_FLOAT_EQ((vel - (points[1] - points[0])).norm(), 0.f); + + // WHEN: we get the end point + EXPECT_TRUE(bezier::calculateBezierPosVel(points, 2, 1.f, pos, vel)); + + // THEN: the position should be the first point, and the velocity should still be the length + EXPECT_EQ((pos - points[1]).norm(), 0.f); + EXPECT_FLOAT_EQ((vel - (points[1] - points[0])).norm(), 0.f); +} + +TEST(BezierN_calculateBezier, works_3_points_zero_accel) +{ + // GIVEN: 3 points bezier, evenly spaced in a straight line + matrix::Vector3f points[4] = {matrix::Vector3f(1, 2, 3), matrix::Vector3f(5, 0, 1), matrix::Vector3f(9, -2, -1), matrix::Vector3f(NAN, NAN, NAN)}; + matrix::Vector3f pos, vel; + pos *= NAN; + vel *= NAN; + + // WHEN: we get the half-way point + EXPECT_TRUE(bezier::calculateBezierPosVel(points, 3, 0.5f, pos, vel)); + + // THEN: it should be the middle point, with velocity of 1st to last + EXPECT_FLOAT_EQ((pos - points[1]).norm(), 0.f); + EXPECT_FLOAT_EQ((vel - (points[2] - points[0])).norm(), 0.f); + + matrix::Vector3f pos2, vel2, accel2; + + // WHEN: we use the accel interface + EXPECT_TRUE(bezier::calculateBezierPosVelAcc(points, 3, 0.5f, pos2, vel2, accel2)); + + // THEN: it should give same position, velocity as the non-accel interface, and zero accel (since this curve is 0 accel) + EXPECT_FLOAT_EQ((pos2 - pos).norm(), 0.f); + EXPECT_FLOAT_EQ((vel2 - vel).norm(), 0.f); + EXPECT_FLOAT_EQ(accel2.norm(), 0.f); + + // WHEN: we check at the beginning + EXPECT_TRUE(bezier::calculateBezierPosVel(points, 3, 0.f, pos, vel)); + EXPECT_TRUE(bezier::calculateBezierPosVelAcc(points, 3, 0.f, pos2, vel2, accel2)); + + // THEN: it should be the starting point and same velocity + EXPECT_FLOAT_EQ((pos - points[0]).norm(), 0.f); + EXPECT_FLOAT_EQ((vel - (points[2] - points[0])).norm(), 0.f); + EXPECT_FLOAT_EQ((pos2 - pos).norm(), 0.f); + EXPECT_FLOAT_EQ((vel2 - vel).norm(), 0.f); + EXPECT_FLOAT_EQ(accel2.norm(), 0.f); + + // WHEN: we check at the end + EXPECT_TRUE(bezier::calculateBezierPosVel(points, 3, 1.f, pos, vel)); + EXPECT_TRUE(bezier::calculateBezierPosVelAcc(points, 3, 1.f, pos2, vel2, accel2)); + + // THEN: it should be the ending point and same velocity + EXPECT_FLOAT_EQ((pos - points[2]).norm(), 0.f); + EXPECT_FLOAT_EQ((vel - (points[2] - points[0])).norm(), 0.f); + EXPECT_FLOAT_EQ((pos2 - pos).norm(), 0.f); + EXPECT_FLOAT_EQ((vel2 - vel).norm(), 0.f); + EXPECT_FLOAT_EQ(accel2.norm(), 0.f); +} + +TEST(BezierN_calculateBezier, works_3_points_accel) +{ + // GIVEN: 3 points bezier, in a curve + matrix::Vector3f points[4] = {matrix::Vector3f(1, 2, 3), matrix::Vector3f(5, 0, 1), matrix::Vector3f(19, -8, 1), matrix::Vector3f(NAN, NAN, NAN)}; + matrix::Vector3f pos, vel; + pos *= NAN; + vel *= NAN; + + matrix::Vector3f pos2; + pos2 *= NAN; + + matrix::Vector3f accel_start, accel_mid, accel_end; + matrix::Vector3f vel_start, vel_mid, vel_end; + + + // WHEN: we check at the beginning + EXPECT_TRUE(bezier::calculateBezierPosVel(points, 3, 0.f, pos, vel)); + EXPECT_TRUE(bezier::calculateBezierPosVelAcc(points, 3, 0.f, pos2, vel_start, accel_start)); + + // THEN: it should give same position, velocity as the non-accel interface, and non-zero accel + EXPECT_FLOAT_EQ((pos2 - pos).norm(), 0.f); + EXPECT_FLOAT_EQ((vel_start - vel).norm(), 0.f); + EXPECT_GT(accel_start.norm(), 0.f); + + // WHEN: we use the accel interface to get the half-way point + EXPECT_TRUE(bezier::calculateBezierPosVel(points, 3, 0.5f, pos, vel)); + EXPECT_TRUE(bezier::calculateBezierPosVelAcc(points, 3, 0.5f, pos2, vel_mid, accel_mid)); + + // THEN: the values should matche between accel and non-accel version + EXPECT_FLOAT_EQ((pos2 - pos).norm(), 0.f); + EXPECT_FLOAT_EQ((vel_mid - vel).norm(), 0.f); + + // AND: the accel should be the same as the start + EXPECT_FLOAT_EQ((accel_mid - accel_start).norm(), 0.f); + + + // WHEN: we check at the end + EXPECT_TRUE(bezier::calculateBezierPosVel(points, 3, 1.f, pos, vel)); + EXPECT_TRUE(bezier::calculateBezierPosVelAcc(points, 3, 1.f, pos2, vel_end, accel_end)); + + // THEN: it should be the ending point, and accel should match + EXPECT_FLOAT_EQ((pos - points[2]).norm(), 0.f); + EXPECT_FLOAT_EQ((pos2 - pos).norm(), 0.f); + EXPECT_FLOAT_EQ((vel_end - vel).norm(), 0.f); + EXPECT_FLOAT_EQ((accel_end - accel_start).norm(), 0.f); + + // FINALLY: mid point velocity should be average of start and end velocity + EXPECT_FLOAT_EQ((vel_mid - 0.5f * (vel_start + vel_end)).norm(), 0.f); +} + +TEST(BezierN_calculateBezierYaw, checks_validity) +{ + float points[10]; + float a, b; + EXPECT_FALSE(bezier::calculateBezierYaw(nullptr, 10, 0.5f, a, b)); + EXPECT_FALSE(bezier::calculateBezierYaw(points, 0, 0.5f, a, b)); + EXPECT_FALSE(bezier::calculateBezierYaw(points, 10, -0.5f, a, b)); + EXPECT_FALSE(bezier::calculateBezierYaw(points, 10, 1.5f, a, b)); +} + +TEST(BezierN_calculateBezierYaw, work_1_point) +{ + // GIVEN: a single yaw point + float points[2] = {M_PI / 2, NAN}; + float yaw, yaw_speed; + + // WHEN: we use it as a 1-point bezier curve + EXPECT_TRUE(bezier::calculateBezierYaw(points, 1, 0.5f, yaw, yaw_speed)); + + // THEN: it should have that same value, and the velocity should be 0 + EXPECT_FLOAT_EQ(yaw, M_PI / 2); + EXPECT_FLOAT_EQ(yaw_speed, 0); +} + +TEST(BezierN_calculateBezierYaw, work_2_points) +{ + // GIVEN: a single yaw point + float points[3] = {0, M_PI / 2, NAN}; + float yaw, yaw_speed; + + // WHEN: we get the beginning + EXPECT_TRUE(bezier::calculateBezierYaw(points, 2, 0.f, yaw, yaw_speed)); + + // THEN: it should have the beginning value, and the velocity should be the difference between first and last + EXPECT_FLOAT_EQ(yaw, 0); + EXPECT_FLOAT_EQ(yaw_speed, M_PI / 2); + + // WHEN: we get the middle + EXPECT_TRUE(bezier::calculateBezierYaw(points, 2, 0.5f, yaw, yaw_speed)); + + // THEN: it should have the beginning value, and the velocity should be the difference between first and last + EXPECT_FLOAT_EQ(yaw, M_PI / 4); + EXPECT_FLOAT_EQ(yaw_speed, M_PI / 2); + + // WHEN: we get the end + EXPECT_TRUE(bezier::calculateBezierYaw(points, 2, 1.f, yaw, yaw_speed)); + + // THEN: it should have the beginning value, and the velocity should be the difference between first and last + EXPECT_FLOAT_EQ(yaw, M_PI / 2); + EXPECT_FLOAT_EQ(yaw_speed, M_PI / 2); +} + +TEST(BezierN_calculateBezierYaw, work_2_points_wrap) +{ + // GIVEN: 2 yaw points on either side of the +- PI wrap line + float points[3] = {-M_PI + 0.1, M_PI - 0.1, NAN}; + float yaw, yaw_speed; + + // WHEN: we get the beginning + EXPECT_TRUE(bezier::calculateBezierYaw(points, 2, 0.f, yaw, yaw_speed)); + + // THEN: it should have the beginning value, and the velocity should be the wrapped distance between first and last + EXPECT_FLOAT_EQ(yaw, -M_PI + 0.1); + EXPECT_NEAR(yaw_speed, -0.2, 1e-6f); + + // WHEN: we get the middle + EXPECT_TRUE(bezier::calculateBezierYaw(points, 2, 0.5f, yaw, yaw_speed)); + + // THEN: it should have the wrapped middle value, and the velocity should be the wrapped distance between first and last + EXPECT_FLOAT_EQ(matrix::wrap_pi(yaw - float(M_PI)), 0); + EXPECT_NEAR(yaw_speed, -0.2, 1e-6f); + + // WHEN: we get the end + EXPECT_TRUE(bezier::calculateBezierYaw(points, 2, 1.f, yaw, yaw_speed)); + + // THEN: it should have the end value, and the velocity should be the wrapped distance between first and last + EXPECT_FLOAT_EQ(yaw, M_PI - 0.1); + EXPECT_NEAR(yaw_speed, -0.2, 1e-6f); +} + + +TEST(BezierN_calculateT, rejects_bad_timestamps) +{ + float f = NAN; + EXPECT_FALSE(bezier::calculateT(100, 1000, 99, f)); + EXPECT_FALSE(bezier::calculateT(100, 1000, 1001, f)); + EXPECT_FALSE(bezier::calculateT(1001, 1000, 1001, f)); +} + + +TEST(BezierN_calculateT, begin_middle_end) +{ + float f = NAN; + EXPECT_TRUE(bezier::calculateT(100, 1000, 100, f)); + EXPECT_FLOAT_EQ(f, 0.f); + + EXPECT_TRUE(bezier::calculateT(100, 1000, 550, f)); + EXPECT_FLOAT_EQ(f, 0.5f); + + EXPECT_TRUE(bezier::calculateT(100, 1000, 1000, f)); + EXPECT_FLOAT_EQ(f, 1.f); +} + +TEST(BezierN_calculateT, giant_offset) +{ + int64_t offset = 0xFFFFFFFFFFFF; // 48 bit max + float f = NAN; + EXPECT_TRUE(bezier::calculateT(offset + 100, offset + 1000, offset + 100, f)); + EXPECT_FLOAT_EQ(f, 0.f); + + EXPECT_TRUE(bezier::calculateT(offset + 100, offset + 1000, offset + 550, f)); + EXPECT_FLOAT_EQ(f, 0.5f); + + EXPECT_TRUE(bezier::calculateT(offset + 100, offset + 1000, offset + 1000, f)); + EXPECT_FLOAT_EQ(f, 1.f); +} diff --git a/src/lib/bezier/BezierQuad.cpp b/src/lib/bezier/BezierQuad.cpp index 1b8d73599f..61309cc496 100644 --- a/src/lib/bezier/BezierQuad.cpp +++ b/src/lib/bezier/BezierQuad.cpp @@ -37,6 +37,12 @@ * @author Dennis Mannhart */ +// The header includes this implementation, so in order to have this library shared with other +// .cpp files and avoid duplication of constants, we need to prevent including it twice +#ifndef BEZIER_QUAD_CPP +#define BEZIER_QUAD_CPP + + #include "BezierQuad.hpp" namespace bezier @@ -213,3 +219,5 @@ Tp BezierQuad::_getDistanceSquared(const Tp t, const Vector3_t &pose) return (vec * vec); } } + +#endif diff --git a/src/lib/bezier/CMakeLists.txt b/src/lib/bezier/CMakeLists.txt index 44c8ff6183..f069fff591 100644 --- a/src/lib/bezier/CMakeLists.txt +++ b/src/lib/bezier/CMakeLists.txt @@ -33,4 +33,7 @@ px4_add_library(bezier BezierQuad.cpp + BezierN.cpp ) + +px4_add_unit_gtest(SRC BezierNTest.cpp LINKLIBS bezier)