moveit2
The MoveIt Motion Planning Framework for ROS 2.
Classes | Functions
unittest_trajectory_generator_circ.cpp File Reference
#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:

Go to the source code of this file.

Classes

class  TrajectoryGeneratorCIRCTest
 

Functions

 TEST_F (TrajectoryGeneratorCIRCTest, TestExceptionErrorCodeMapping)
 Checks that each derived MoveItErrorCodeException contains the correct error code. More...
 
 TEST_F (TrajectoryGeneratorCIRCTest, noLimits)
 Construct a TrajectoryGeneratorCirc with no limits given. 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 constrainst. More...
 
 TEST_F (TrajectoryGeneratorCIRCTest, CenterPointPoseGoalFrameIdOrientationConstraints)
 Set a frame id only on the orientation constrainst. 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 769 of file unittest_trajectory_generator_circ.cpp.

◆ TEST_F() [1/28]

TEST_F ( TrajectoryGeneratorCIRCTest  ,
accScaleToHigh   
)

Generate invalid circ with to high acc scaling.

Definition at line 345 of file unittest_trajectory_generator_circ.cpp.

◆ TEST_F() [2/28]

TEST_F ( TrajectoryGeneratorCIRCTest  ,
centerPointJointGoal   
)

test the circ planner with center point and joint goal

Definition at line 593 of file unittest_trajectory_generator_circ.cpp.

◆ TEST_F() [3/28]

TEST_F ( TrajectoryGeneratorCIRCTest  ,
CenterPointPoseGoal   
)

test the circ planner with center point and pose goal

Definition at line 655 of file unittest_trajectory_generator_circ.cpp.

◆ TEST_F() [4/28]

TEST_F ( TrajectoryGeneratorCIRCTest  ,
CenterPointPoseGoalFrameIdBothConstraints   
)

Set a frame_id on both position and orientation constraints.

Definition at line 703 of file unittest_trajectory_generator_circ.cpp.

◆ TEST_F() [5/28]

TEST_F ( TrajectoryGeneratorCIRCTest  ,
CenterPointPoseGoalFrameIdOrientationConstraints   
)

Set a frame id only on the orientation constrainst.

Definition at line 687 of file unittest_trajectory_generator_circ.cpp.

◆ TEST_F() [6/28]

TEST_F ( TrajectoryGeneratorCIRCTest  ,
CenterPointPoseGoalFrameIdPositionConstraints   
)

Set a frame id only on the position constrainst.

Definition at line 670 of file unittest_trajectory_generator_circ.cpp.

◆ TEST_F() [7/28]

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 468 of file unittest_trajectory_generator_circ.cpp.

◆ TEST_F() [8/28]

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 532 of file unittest_trajectory_generator_circ.cpp.

◆ TEST_F() [9/28]

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 512 of file unittest_trajectory_generator_circ.cpp.

◆ TEST_F() [10/28]

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 489 of file unittest_trajectory_generator_circ.cpp.

◆ TEST_F() [11/28]

TEST_F ( TrajectoryGeneratorCIRCTest  ,
emptyAux   
)

test invalid motion plan request with no aux point defined

Definition at line 403 of file unittest_trajectory_generator_circ.cpp.

◆ TEST_F() [12/28]

TEST_F ( TrajectoryGeneratorCIRCTest  ,
incompleteStartState   
)

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

Definition at line 291 of file unittest_trajectory_generator_circ.cpp.

◆ TEST_F() [13/28]

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 565 of file unittest_trajectory_generator_circ.cpp.

◆ TEST_F() [14/28]

TEST_F ( TrajectoryGeneratorCIRCTest  ,
InterimPointJointGoal   
)

test the circ planner with interim point with joint goal

Definition at line 722 of file unittest_trajectory_generator_circ.cpp.

◆ TEST_F() [15/28]

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 740 of file unittest_trajectory_generator_circ.cpp.

◆ TEST_F() [16/28]

TEST_F ( TrajectoryGeneratorCIRCTest  ,
InterimPointPoseGoal   
)

test the circ planner with interim point with pose goal

Definition at line 758 of file unittest_trajectory_generator_circ.cpp.

◆ TEST_F() [17/28]

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 609 of file unittest_trajectory_generator_circ.cpp.

◆ TEST_F() [18/28]

TEST_F ( TrajectoryGeneratorCIRCTest  ,
invalidAuxLinkName   
)

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

Definition at line 436 of file unittest_trajectory_generator_circ.cpp.

◆ TEST_F() [19/28]

TEST_F ( TrajectoryGeneratorCIRCTest  ,
invalidAuxName   
)

test invalid motion plan request with no aux name defined

Definition at line 419 of file unittest_trajectory_generator_circ.cpp.

◆ TEST_F() [20/28]

TEST_F ( TrajectoryGeneratorCIRCTest  ,
invalidCenter   
)

test the circ planner with invalid center point

Definition at line 452 of file unittest_trajectory_generator_circ.cpp.

◆ TEST_F() [21/28]

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 636 of file unittest_trajectory_generator_circ.cpp.

◆ TEST_F() [22/28]

TEST_F ( TrajectoryGeneratorCIRCTest  ,
noLimits   
)

Construct a TrajectoryGeneratorCirc with no limits given.

Definition at line 280 of file unittest_trajectory_generator_circ.cpp.

◆ TEST_F() [23/28]

TEST_F ( TrajectoryGeneratorCIRCTest  ,
nonZeroStartVelocity   
)

test invalid motion plan request with non zero start velocity

Definition at line 308 of file unittest_trajectory_generator_circ.cpp.

◆ TEST_F() [24/28]

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 359 of file unittest_trajectory_generator_circ.cpp.

◆ TEST_F() [25/28]

TEST_F ( TrajectoryGeneratorCIRCTest  ,
samePointsWithInterim   
)

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

Expected: Planning should fail.

Definition at line 382 of file unittest_trajectory_generator_circ.cpp.

◆ TEST_F() [26/28]

TEST_F ( TrajectoryGeneratorCIRCTest  ,
TestExceptionErrorCodeMapping   
)

Checks that each derived MoveItErrorCodeException contains the correct error code.

Definition at line 219 of file unittest_trajectory_generator_circ.cpp.

◆ TEST_F() [27/28]

TEST_F ( TrajectoryGeneratorCIRCTest  ,
ValidCommand   
)

Definition at line 320 of file unittest_trajectory_generator_circ.cpp.

◆ TEST_F() [28/28]

TEST_F ( TrajectoryGeneratorCIRCTest  ,
velScaleToHigh   
)

Generate invalid circ with to high vel scaling.

Definition at line 332 of file unittest_trajectory_generator_circ.cpp.