#include <memory>
#include <gtest/gtest.h>
#include <moveit/kinematic_constraints/utils.h>
#include <moveit/robot_model/robot_model.h>
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/robot_state/conversions.h>
#include <tf2_eigen/tf2_eigen.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <pilz_industrial_motion_planner/joint_limits_aggregator.h>
#include <pilz_industrial_motion_planner/trajectory_generator_circ.h>
#include <pilz_industrial_motion_planner_testutils/command_types_typedef.h>
#include <pilz_industrial_motion_planner_testutils/xml_testdata_loader.h>
#include "test_utils.h"
#include <rclcpp/rclcpp.hpp>

Include dependency graph for unittest_trajectory_generator_circ.cpp:

Classes
class	TrajectoryGeneratorCIRCTest

Functions
	TEST_F (TrajectoryGeneratorCIRCTest, TestExceptionErrorCodeMapping)
	Checks that each derived MoveItErrorCodeException contains the correct error code. More...

	TEST_F (TrajectoryGeneratorCIRCTest, incompleteStartState)
	test invalid motion plan request with incomplete start state and cartesian goal More...

	TEST_F (TrajectoryGeneratorCIRCTest, nonZeroStartVelocity)
	test invalid motion plan request with non zero start velocity More...

	TEST_F (TrajectoryGeneratorCIRCTest, ValidCommand)

	TEST_F (TrajectoryGeneratorCIRCTest, velScaleToHigh)
	Generate invalid circ with to high vel scaling. More...

	TEST_F (TrajectoryGeneratorCIRCTest, accScaleToHigh)
	Generate invalid circ with to high acc scaling. More...

	TEST_F (TrajectoryGeneratorCIRCTest, samePointsWithCenter)
	Use three points (with center) with a really small distance between to trigger a internal throw from KDL. More...

	TEST_F (TrajectoryGeneratorCIRCTest, samePointsWithInterim)
	Use three points (with interim) with a really small distance between. More...

	TEST_F (TrajectoryGeneratorCIRCTest, emptyAux)
	test invalid motion plan request with no aux point defined More...

	TEST_F (TrajectoryGeneratorCIRCTest, invalidAuxName)
	test invalid motion plan request with no aux name defined More...

	TEST_F (TrajectoryGeneratorCIRCTest, invalidAuxLinkName)
	test invalid motion plan request with invalid link name in the auxiliary point More...

	TEST_F (TrajectoryGeneratorCIRCTest, invalidCenter)
	test the circ planner with invalid center point More...

	TEST_F (TrajectoryGeneratorCIRCTest, colinearCenter)
	test the circ planner with colinear start/goal/center position More...

	TEST_F (TrajectoryGeneratorCIRCTest, colinearInterim)
	test the circ planner with colinear start/goal/interim position More...

	TEST_F (TrajectoryGeneratorCIRCTest, colinearCenterDueToInterim)
	test the circ planner with half circle with interim point More...

	TEST_F (TrajectoryGeneratorCIRCTest, colinearCenterAndInterim)
	test the circ planner with colinear start/center/interim positions More...

	TEST_F (TrajectoryGeneratorCIRCTest, interimLarger180Degree)
	test the circ planner with a circ path where the angle between goal and interim is larger than 180 degree More...

	TEST_F (TrajectoryGeneratorCIRCTest, centerPointJointGoal)
	test the circ planner with center point and joint goal More...

	TEST_F (TrajectoryGeneratorCIRCTest, InvalidAdditionalPrimitivePose)
	A valid circ request contains a helping point (interim or center), in this test a additional point is defined as an invalid test case. More...

	TEST_F (TrajectoryGeneratorCIRCTest, InvalidExtraJointConstraint)
	Joint Goals are expected to match the start state in number and joint_names Here an additional joint constraints is "falsely" defined to check for the error. More...

	TEST_F (TrajectoryGeneratorCIRCTest, CenterPointPoseGoal)
	test the circ planner with center point and pose goal More...

	TEST_F (TrajectoryGeneratorCIRCTest, CenterPointPoseGoalFrameIdPositionConstraints)
	Set a frame id only on the position constraints. More...

	TEST_F (TrajectoryGeneratorCIRCTest, CenterPointPoseGoalFrameIdOrientationConstraints)
	Set a frame id only on the orientation constraints. More...

	TEST_F (TrajectoryGeneratorCIRCTest, CenterPointPoseGoalFrameIdBothConstraints)
	Set a frame_id on both position and orientation constraints. More...

	TEST_F (TrajectoryGeneratorCIRCTest, InterimPointJointGoal)
	test the circ planner with interim point with joint goal More...

	TEST_F (TrajectoryGeneratorCIRCTest, InterimPointJointGoalStartVelNearZero)
	test the circ planner with interim point with joint goal and a close to zero velocity of the start state More...

	TEST_F (TrajectoryGeneratorCIRCTest, InterimPointPoseGoal)
	test the circ planner with interim point with pose goal More...

int	main (int argc, char **argv)

Function Documentation

◆ main()

int main	(	int	argc,
		char **	argv
	)

Definition at line 760 of file unittest_trajectory_generator_circ.cpp.

◆ TEST_F() [1/27]

TEST_F	(	TrajectoryGeneratorCIRCTest	,
		accScaleToHigh
	)

Generate invalid circ with to high acc scaling.

Definition at line 336 of file unittest_trajectory_generator_circ.cpp.

◆ TEST_F() [2/27]

TEST_F	(	TrajectoryGeneratorCIRCTest	,
		centerPointJointGoal
	)

test the circ planner with center point and joint goal

Definition at line 584 of file unittest_trajectory_generator_circ.cpp.

◆ TEST_F() [3/27]

TEST_F	(	TrajectoryGeneratorCIRCTest	,
		CenterPointPoseGoal
	)

test the circ planner with center point and pose goal

Definition at line 646 of file unittest_trajectory_generator_circ.cpp.

◆ TEST_F() [4/27]

TEST_F	(	TrajectoryGeneratorCIRCTest	,
		CenterPointPoseGoalFrameIdBothConstraints
	)

Set a frame_id on both position and orientation constraints.

Definition at line 694 of file unittest_trajectory_generator_circ.cpp.

◆ TEST_F() [5/27]

TEST_F	(	TrajectoryGeneratorCIRCTest	,
		CenterPointPoseGoalFrameIdOrientationConstraints
	)

Set a frame id only on the orientation constraints.

Definition at line 678 of file unittest_trajectory_generator_circ.cpp.

◆ TEST_F() [6/27]

TEST_F	(	TrajectoryGeneratorCIRCTest	,
		CenterPointPoseGoalFrameIdPositionConstraints
	)

Set a frame id only on the position constraints.

Definition at line 661 of file unittest_trajectory_generator_circ.cpp.

◆ TEST_F() [7/27]

TEST_F	(	TrajectoryGeneratorCIRCTest	,
		colinearCenter
	)

test the circ planner with colinear start/goal/center position

Expected: Planning should fail since the path is not uniquely defined.

Definition at line 459 of file unittest_trajectory_generator_circ.cpp.

◆ TEST_F() [8/27]

TEST_F	(	TrajectoryGeneratorCIRCTest	,
		colinearCenterAndInterim
	)

test the circ planner with colinear start/center/interim positions

The request contains start/interim/goal such that start, center (not explicitly given) and interim are colinear. In case the interim is used as auxiliary point for KDL::Path_Circle this should fail.

Expected: Planning should successfully return.

Definition at line 523 of file unittest_trajectory_generator_circ.cpp.

◆ TEST_F() [9/27]

TEST_F	(	TrajectoryGeneratorCIRCTest	,
		colinearCenterDueToInterim
	)

test the circ planner with half circle with interim point

The request contains start/interim/goal such that start, center (not explicitly given) and goal are colinear

Expected: Planning should successfully return.

Definition at line 503 of file unittest_trajectory_generator_circ.cpp.

◆ TEST_F() [10/27]

TEST_F	(	TrajectoryGeneratorCIRCTest	,
		colinearInterim
	)

test the circ planner with colinear start/goal/interim position

Expected: Planning should fail. These positions do not even represent a circle.

Definition at line 480 of file unittest_trajectory_generator_circ.cpp.

◆ TEST_F() [11/27]

TEST_F	(	TrajectoryGeneratorCIRCTest	,
		emptyAux
	)

test invalid motion plan request with no aux point defined

Definition at line 394 of file unittest_trajectory_generator_circ.cpp.

◆ TEST_F() [12/27]

TEST_F	(	TrajectoryGeneratorCIRCTest	,
		incompleteStartState
	)

test invalid motion plan request with incomplete start state and cartesian goal

Definition at line 282 of file unittest_trajectory_generator_circ.cpp.

◆ TEST_F() [13/27]

TEST_F	(	TrajectoryGeneratorCIRCTest	,
		interimLarger180Degree
	)

test the circ planner with a circ path where the angle between goal and interim is larger than 180 degree

The request contains start/interim/goal such that 180 degree < interim angle < goal angle.

Expected: Planning should successfully return.

Definition at line 556 of file unittest_trajectory_generator_circ.cpp.

◆ TEST_F() [14/27]

TEST_F	(	TrajectoryGeneratorCIRCTest	,
		InterimPointJointGoal
	)

test the circ planner with interim point with joint goal

Definition at line 713 of file unittest_trajectory_generator_circ.cpp.

◆ TEST_F() [15/27]

TEST_F	(	TrajectoryGeneratorCIRCTest	,
		InterimPointJointGoalStartVelNearZero
	)

test the circ planner with interim point with joint goal and a close to zero velocity of the start state

The generator is expected to be robust against a velocity being almost zero.

Definition at line 731 of file unittest_trajectory_generator_circ.cpp.

◆ TEST_F() [16/27]

TEST_F	(	TrajectoryGeneratorCIRCTest	,
		InterimPointPoseGoal
	)

test the circ planner with interim point with pose goal

Definition at line 749 of file unittest_trajectory_generator_circ.cpp.

◆ TEST_F() [17/27]

TEST_F	(	TrajectoryGeneratorCIRCTest	,
		InvalidAdditionalPrimitivePose
	)

A valid circ request contains a helping point (interim or center), in this test a additional point is defined as an invalid test case.

Definition at line 600 of file unittest_trajectory_generator_circ.cpp.

◆ TEST_F() [18/27]

TEST_F	(	TrajectoryGeneratorCIRCTest	,
		invalidAuxLinkName
	)

test invalid motion plan request with invalid link name in the auxiliary point

Definition at line 427 of file unittest_trajectory_generator_circ.cpp.

◆ TEST_F() [19/27]

TEST_F	(	TrajectoryGeneratorCIRCTest	,
		invalidAuxName
	)

test invalid motion plan request with no aux name defined

Definition at line 410 of file unittest_trajectory_generator_circ.cpp.

◆ TEST_F() [20/27]

TEST_F	(	TrajectoryGeneratorCIRCTest	,
		invalidCenter
	)

test the circ planner with invalid center point

Definition at line 443 of file unittest_trajectory_generator_circ.cpp.

◆ TEST_F() [21/27]

TEST_F	(	TrajectoryGeneratorCIRCTest	,
		InvalidExtraJointConstraint
	)

Joint Goals are expected to match the start state in number and joint_names Here an additional joint constraints is "falsely" defined to check for the error.

Definition at line 627 of file unittest_trajectory_generator_circ.cpp.

◆ TEST_F() [22/27]

TEST_F	(	TrajectoryGeneratorCIRCTest	,
		nonZeroStartVelocity
	)

test invalid motion plan request with non zero start velocity

Definition at line 299 of file unittest_trajectory_generator_circ.cpp.

◆ TEST_F() [23/27]

TEST_F	(	TrajectoryGeneratorCIRCTest	,
		samePointsWithCenter
	)

Use three points (with center) with a really small distance between to trigger a internal throw from KDL.

Definition at line 350 of file unittest_trajectory_generator_circ.cpp.

◆ TEST_F() [24/27]

TEST_F	(	TrajectoryGeneratorCIRCTest	,
		samePointsWithInterim
	)

Use three points (with interim) with a really small distance between.

Expected: Planning should fail.

Definition at line 373 of file unittest_trajectory_generator_circ.cpp.

◆ TEST_F() [25/27]

TEST_F	(	TrajectoryGeneratorCIRCTest	,
		TestExceptionErrorCodeMapping
	)

Checks that each derived MoveItErrorCodeException contains the correct error code.

Definition at line 220 of file unittest_trajectory_generator_circ.cpp.

◆ TEST_F() [26/27]

TEST_F	(	TrajectoryGeneratorCIRCTest	,
		ValidCommand
	)

Definition at line 311 of file unittest_trajectory_generator_circ.cpp.

◆ TEST_F() [27/27]

TEST_F	(	TrajectoryGeneratorCIRCTest	,
		velScaleToHigh
	)

Generate invalid circ with to high vel scaling.

Definition at line 323 of file unittest_trajectory_generator_circ.cpp.

Classes

Functions

Function Documentation

◆ main()

◆ TEST_F() [1/27]

◆ TEST_F() [2/27]

◆ TEST_F() [3/27]

◆ TEST_F() [4/27]

◆ TEST_F() [5/27]

◆ TEST_F() [6/27]

◆ TEST_F() [7/27]

◆ TEST_F() [8/27]

◆ TEST_F() [9/27]

◆ TEST_F() [10/27]

◆ TEST_F() [11/27]

◆ TEST_F() [12/27]

◆ TEST_F() [13/27]

◆ TEST_F() [14/27]

◆ TEST_F() [15/27]

◆ TEST_F() [16/27]

◆ TEST_F() [17/27]

◆ TEST_F() [18/27]

◆ TEST_F() [19/27]

◆ TEST_F() [20/27]

◆ TEST_F() [21/27]

◆ TEST_F() [22/27]

◆ TEST_F() [23/27]

◆ TEST_F() [24/27]

◆ TEST_F() [25/27]

◆ TEST_F() [26/27]

◆ TEST_F() [27/27]