test_catmull_rom_spline.cpp File Reference

#include <gtest/gtest.h>
#include <geometry/quaternion/euler_to_quaternion.hpp>
#include <geometry/spline/catmull_rom_spline.hpp>
#include <scenario_simulator_exception/exception.hpp>
#include "expect_eq_macros.hpp"
#include "test_utils.hpp"

Include dependency graph for test_catmull_rom_spline.cpp:

Functions
math::geometry::CatmullRomSpline	makeLine ()
	Helper function generating line: p(0,0)-> p(1,3) -> p(2,6) More...
math::geometry::CatmullRomSpline	makeSlopedLine ()
	Helper function generating sloped line: p(0,0,0)-> p(1,3,1) -> p(2,6,2) More...
math::geometry::CatmullRomSpline	makeCurve ()
	Helper function generating curve: p(0,0)-> p(1,1)-> p(2,0) More...
math::geometry::CatmullRomSpline	makeCurve2 ()
	Helper function generating curve: p(0,0)-> p(1,1)-> p(0,2) More...
void	addOffset (geometry_msgs::msg::Point &point, const double offset, const double theta)
	TEST (CatmullRomSpline, GetCollisionWith2ControlPoints)
	Testing the CatmullRomSpline::getCollisionPointIn2D function works valid. In this test case, number of the control points of the catmull-rom spline (variable name spline) is 2, so the shape of the value spline is line segment. More...
	TEST (CatmullRomSpline, GetCollisionWith1ControlPoint)
	Testing the CatmullRomSpline::getCollisionPointIn2D function works valid In this test case, number of the control points of the catmull-rom spline (variable name spline) is 1, so the shape of the value spline is single point. More...
	TEST (CatmullRomSpline, GetCollisionPointIn2D)
	TEST (CatmullRomSpline, getCollisionPointIn2D)
	TEST (CatmullRomSpline, getCollisionPointIn2DNoCollision)
	TEST (CatmullRomSpline, getCollisionPointIn2DPolygon)
	TEST (CatmullRomSpline, getCollisionPointIn2DEmpty)
	TEST (CatmullRomSpline, getMaximum2DCurvatureLine)
	TEST (CatmullRomSpline, getMaximum2DCurvatureCurve)
	TEST (CatmullRomSpline, getPolygon)
	TEST (CatmullRomSpline, getPolygonEdge)
	TEST (CatmullRomSpline, initializationLine)
	TEST (CatmullRomSpline, initializationCurve)
	TEST (CatmullRomSpline, getLengthLine)
	TEST (CatmullRomSpline, getLengthCurve)
	TEST (CatmullRomSpline, getPointLine)
	TEST (CatmullRomSpline, getPointCurve)
	TEST (CatmullRomSpline, getTangentVectorLine)
	TEST (CatmullRomSpline, getTangentVectorCurve)
	TEST (CatmullRomSpline, getNormalVectorLine)
	TEST (CatmullRomSpline, getNormalVectorCurve)
	TEST (CatmullRomSpline, getPoseLine)
	TEST (CatmullRomSpline, getPoseLineWithPitch)
	TEST (CatmullRomSpline, getPoseCurve)
	TEST (CatmullRomSpline, getPoseCurveWithPitch)
	TEST (CatmullRomSpline, getSValue1)
	TEST (CatmullRomSpline, getSValue2)
	TEST (CatmullRomSpline, getSValueEdge)
	TEST (CatmullRomSpline, getSquaredDistanceIn2D)
	TEST (CatmullRomSpline, getSquaredDistanceIn2DSamePoint)
	TEST (CatmullRomSpline, getSquaredDistanceVector)
	TEST (CatmullRomSpline, getSquaredDistanceVectorSamePoint)
	TEST (CatmullRomSpline, getTrajectoryLine)
	TEST (CatmullRomSpline, getTrajectoryCurve)
	TEST (CatmullRomSpline, getTrajectoryEmpty)
	TEST (CatmullRomSpline, initializationNotEnoughControlPoints)
int	main (int argc, char **argv)

Variables
constexpr double	EPS = 1e-6

Function Documentation

addOffset()

void addOffset	(	geometry_msgs::msg::Point &	point,
		const double	offset,
		const double	theta
	)

Add an offset to the given point in a specified direction.

Parameters

point	The point to which the offset will be added.
offset	The value of the offset.
theta	The angle in radians representing the direction.

main()

int main	(	int	argc,
		char **	argv
	)

makeCurve()

math::geometry::CatmullRomSpline makeCurve

(

)

Helper function generating curve: p(0,0)-> p(1,1)-> p(2,0)

makeCurve2()

math::geometry::CatmullRomSpline makeCurve2

(

)

Helper function generating curve: p(0,0)-> p(1,1)-> p(0,2)

makeLine()

math::geometry::CatmullRomSpline makeLine

(

)

Helper function generating line: p(0,0)-> p(1,3) -> p(2,6)

makeSlopedLine()

math::geometry::CatmullRomSpline makeSlopedLine

(

)

Helper function generating sloped line: p(0,0,0)-> p(1,3,1) -> p(2,6,2)

TEST() [1/36]

TEST	(	CatmullRomSpline	,
		GetCollisionPointIn2D
	)

TEST() [2/36]

TEST	(	CatmullRomSpline	,
		getCollisionPointIn2D
	)

TEST() [3/36]

TEST	(	CatmullRomSpline	,
		getCollisionPointIn2DEmpty
	)

TEST() [4/36]

TEST	(	CatmullRomSpline	,
		getCollisionPointIn2DNoCollision
	)

TEST() [5/36]

TEST	(	CatmullRomSpline	,
		getCollisionPointIn2DPolygon
	)

TEST() [6/36]

TEST	(	CatmullRomSpline	,
		GetCollisionWith1ControlPoint
	)

Testing the CatmullRomSpline::getCollisionPointIn2D function works valid In this test case, number of the control points of the catmull-rom spline (variable name spline) is 1, so the shape of the value spline is single point.

Note

The variable spline has control point with point (x,y,z) = (0,1,0) in the cartesian coordinate system. So, spline is same as point (x,y,z) = (0,1,0).

Testing the CatmullRomSpline::getCollisionPointIn2D function can find the collision point with spline and a line segment with start point (x,y,z) = (0,1,0) and end point (x,y,z) = (0,-1,0). collision_s variable in the test case is the denormalized s value in frenet coordinate along the spline curve. The collision point is in (x,y,z) = (0,0,0) in the cartesian coordinate system and the point is in s=0.0, So the return value of the CatmullRomSpline::getCollisionPointIn2D function (variable name collision_s) should be std::optional<double>(0.0)

    const auto collision_s = spline.getCollisionPointIn2D(
      {geometry_msgs::build<geometry_msgs::msg::Point>().x(0).y(1).z(0),
       geometry_msgs::build<geometry_msgs::msg::Point>().x(0).y(-1).z(0)});
    EXPECT_TRUE(collision_s);
    if (collision_s) {
      EXPECT_DOUBLE_EQ(collision_s.value(), 0.0);
    }

Note

Testing the CatmullRomSpline::getCollisionPointIn2D function can find that the spline and a line segment with start point (x,y,z) = (1,1,0) and end point (x,y,z) = (1,-1,0) does not collide. If CatmullRomSpline::getCollisionPointIn2D function works expected, it returns std::nullopt;

    EXPECT_FALSE(spline.getCollisionPointIn2D(
      {geometry_msgs::build<geometry_msgs::msg::Point>().x(1).y(1).z(0),
       geometry_msgs::build<geometry_msgs::msg::Point>().x(1).y(-1).z(0)}));

TEST() [7/36]

TEST	(	CatmullRomSpline	,
		GetCollisionWith2ControlPoints
	)

Testing the CatmullRomSpline::getCollisionPointIn2D function works valid. In this test case, number of the control points of the catmull-rom spline (variable name spline) is 2, so the shape of the value spline is line segment.

Note

The spline has control points p0 and p1. Control point p0 is point (x,y,z) = (0,0,0) and control point p1 is point (x,y,z) = (1,0,0) in the cartesian coordinate system.

  geometry_msgs::msg::Point p0;
  geometry_msgs::msg::Point p1;
  p1.x = 1;
  auto points = {p0, p1};
  auto spline = math::geometry::CatmullRomSpline(points);
  EXPECT_DOUBLE_EQ(spline.getLength(), 1);

Note

Testing the CatmullRomSpline::getCollisionPointIn2D function can find the collision point with spline and a line segment with start point (x,y,z) = (0,1,0) and end point (x,y,z) = (0,-1,0). collision_s variable in the test case is the denormalized s value in frenet coordinate along the spline curve. The collision point is in (x,y,z) = (0,0,0) in the cartesian coordinate system and the point is in s=0.0, So the return value of the CatmullRomSpline::getCollisionPointIn2D function (variable name collision_s) should be std::optional<double>(0.0)

    const auto collision_s = spline.getCollisionPointIn2D(
      {geometry_msgs::build<geometry_msgs::msg::Point>().x(0).y(1).z(0),
       geometry_msgs::build<geometry_msgs::msg::Point>().x(0).y(-1).z(0)});
    EXPECT_TRUE(collision_s);
    if (collision_s) {
      EXPECT_DOUBLE_EQ(collision_s.value(), 0.0);
    }

Note

Testing the CatmullRomSpline::getCollisionPointIn2D function can find the collision point with spline and a line segment with start point (x,y,z) = (1,1,0) and end point (x,y,z) = (-1,-1,0). collision_s variable in the test case is the denormalized s value in frenet coordinate along the spline curve. The collision point is in (x,y,z) = (0,0,0) in the cartesian coordinate system and the point is in s=0.0, So the return value of the CatmullRomSpline::getCollisionPointIn2D function (variable name collision_s) should be std::optional<double>(0.0)

    const auto collision_s = spline.getCollisionPointIn2D(
      {geometry_msgs::build<geometry_msgs::msg::Point>().x(1).y(1).z(0),
       geometry_msgs::build<geometry_msgs::msg::Point>().x(-1).y(-1).z(0)});
    EXPECT_TRUE(collision_s);
    if (collision_s) {
      EXPECT_DOUBLE_EQ(collision_s.value(), 0.0);
    }

Note

Testing the CatmullRomSpline::getCollisionPointIn2D function can find the collision point with spline and a line segment with start point (x,y,z) = (1,1,0) and end point (x,y,z) = (0,-1,0). collision_s variable in the test case is the denormalized s value in frenet coordinate along the spline curve. The collision point is in (x,y,z) = (0,0.5,0) in the cartesian coordinate system and the point is in s=0.5, So the return value of the CatmullRomSpline::getCollisionPointIn2D function (variable name collision_s) should be std::optional<double>(0.5)

    const auto collision_s = spline.getCollisionPointIn2D(
      {geometry_msgs::build<geometry_msgs::msg::Point>().x(1).y(1).z(0),
       geometry_msgs::build<geometry_msgs::msg::Point>().x(0).y(-1).z(0)});
    EXPECT_TRUE(collision_s);
    if (collision_s) {
      EXPECT_DOUBLE_EQ(collision_s.value(), 0.5);
    }

Note

Testing the CatmullRomSpline::getCollisionPointIn2D function can find that the spline and a line segment with start point (x,y,z) = (0,1,0) and end point (x,y,z) = (0,0.2,0) does not collide. If CatmullRomSpline::getCollisionPointIn2D function works expected, it returns std::nullopt;

    const auto collision_s = spline.getCollisionPointIn2D(
      {geometry_msgs::build<geometry_msgs::msg::Point>().x(0).y(1).z(0),
       geometry_msgs::build<geometry_msgs::msg::Point>().x(0).y(0.2).z(0)});
    EXPECT_FALSE(collision_s);

TEST() [8/36]

TEST	(	CatmullRomSpline	,
		getLengthCurve
	)

TEST() [9/36]

TEST	(	CatmullRomSpline	,
		getLengthLine
	)

TEST() [10/36]

TEST	(	CatmullRomSpline	,
		getMaximum2DCurvatureCurve
	)

TEST() [11/36]

TEST	(	CatmullRomSpline	,
		getMaximum2DCurvatureLine
	)

TEST() [12/36]

TEST	(	CatmullRomSpline	,
		getNormalVectorCurve
	)

TEST() [13/36]

TEST	(	CatmullRomSpline	,
		getNormalVectorLine
	)

TEST() [14/36]

TEST	(	CatmullRomSpline	,
		getPointCurve
	)

TEST() [15/36]

TEST	(	CatmullRomSpline	,
		getPointLine
	)

TEST() [16/36]

TEST	(	CatmullRomSpline	,
		getPolygon
	)

TEST() [17/36]

TEST	(	CatmullRomSpline	,
		getPolygonEdge
	)

TEST() [18/36]

TEST	(	CatmullRomSpline	,
		getPoseCurve
	)

TEST() [19/36]

TEST	(	CatmullRomSpline	,
		getPoseCurveWithPitch
	)

TEST() [20/36]

TEST	(	CatmullRomSpline	,
		getPoseLine
	)

TEST() [21/36]

TEST	(	CatmullRomSpline	,
		getPoseLineWithPitch
	)

TEST() [22/36]

TEST	(	CatmullRomSpline	,
		getSquaredDistanceIn2D
	)

TEST() [23/36]

TEST	(	CatmullRomSpline	,
		getSquaredDistanceIn2DSamePoint
	)

TEST() [24/36]

TEST	(	CatmullRomSpline	,
		getSquaredDistanceVector
	)

TEST() [25/36]

TEST	(	CatmullRomSpline	,
		getSquaredDistanceVectorSamePoint
	)

TEST() [26/36]

TEST	(	CatmullRomSpline	,
		getSValue1
	)

TEST() [27/36]

TEST	(	CatmullRomSpline	,
		getSValue2
	)

TEST() [28/36]

TEST	(	CatmullRomSpline	,
		getSValueEdge
	)

TEST() [29/36]

TEST	(	CatmullRomSpline	,
		getTangentVectorCurve
	)

TEST() [30/36]

TEST	(	CatmullRomSpline	,
		getTangentVectorLine
	)

TEST() [31/36]

TEST	(	CatmullRomSpline	,
		getTrajectoryCurve
	)

TEST() [32/36]

TEST	(	CatmullRomSpline	,
		getTrajectoryEmpty
	)

TEST() [33/36]

TEST	(	CatmullRomSpline	,
		getTrajectoryLine
	)

TEST() [34/36]

TEST	(	CatmullRomSpline	,
		initializationCurve
	)

TEST() [35/36]

TEST	(	CatmullRomSpline	,
		initializationLine
	)

TEST() [36/36]

TEST	(	CatmullRomSpline	,
		initializationNotEnoughControlPoints
	)

Variable Documentation

EPS

constexpr double EPS = 1e-6

constexpr