moveit2
The MoveIt Motion Planning Framework for ROS 2.
|
#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>
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. | |
TEST_F (TrajectoryGeneratorCIRCTest, incompleteStartState) | |
test invalid motion plan request with incomplete start state and cartesian goal | |
TEST_F (TrajectoryGeneratorCIRCTest, nonZeroStartVelocity) | |
test invalid motion plan request with non zero start velocity | |
TEST_F (TrajectoryGeneratorCIRCTest, ValidCommand) | |
TEST_F (TrajectoryGeneratorCIRCTest, velScaleToHigh) | |
Generate invalid circ with to high vel scaling. | |
TEST_F (TrajectoryGeneratorCIRCTest, accScaleToHigh) | |
Generate invalid circ with to high acc scaling. | |
TEST_F (TrajectoryGeneratorCIRCTest, samePointsWithCenter) | |
Use three points (with center) with a really small distance between to trigger a internal throw from KDL. | |
TEST_F (TrajectoryGeneratorCIRCTest, samePointsWithInterim) | |
Use three points (with interim) with a really small distance between. | |
TEST_F (TrajectoryGeneratorCIRCTest, emptyAux) | |
test invalid motion plan request with no aux point defined | |
TEST_F (TrajectoryGeneratorCIRCTest, invalidAuxName) | |
test invalid motion plan request with no aux name defined | |
TEST_F (TrajectoryGeneratorCIRCTest, invalidAuxLinkName) | |
test invalid motion plan request with invalid link name in the auxiliary point | |
TEST_F (TrajectoryGeneratorCIRCTest, invalidCenter) | |
test the circ planner with invalid center point | |
TEST_F (TrajectoryGeneratorCIRCTest, colinearCenter) | |
test the circ planner with colinear start/goal/center position | |
TEST_F (TrajectoryGeneratorCIRCTest, colinearInterim) | |
test the circ planner with colinear start/goal/interim position | |
TEST_F (TrajectoryGeneratorCIRCTest, colinearCenterDueToInterim) | |
test the circ planner with half circle with interim point | |
TEST_F (TrajectoryGeneratorCIRCTest, colinearCenterAndInterim) | |
test the circ planner with colinear start/center/interim positions | |
TEST_F (TrajectoryGeneratorCIRCTest, interimLarger180Degree) | |
test the circ planner with a circ path where the angle between goal and interim is larger than 180 degree | |
TEST_F (TrajectoryGeneratorCIRCTest, centerPointJointGoal) | |
test the circ planner with center point and joint goal | |
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. | |
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. | |
TEST_F (TrajectoryGeneratorCIRCTest, CenterPointPoseGoal) | |
test the circ planner with center point and pose goal | |
TEST_F (TrajectoryGeneratorCIRCTest, CenterPointPoseGoalFrameIdPositionConstraints) | |
Set a frame id only on the position constraints. | |
TEST_F (TrajectoryGeneratorCIRCTest, CenterPointPoseGoalFrameIdOrientationConstraints) | |
Set a frame id only on the orientation constraints. | |
TEST_F (TrajectoryGeneratorCIRCTest, CenterPointPoseGoalFrameIdBothConstraints) | |
Set a frame_id on both position and orientation constraints. | |
TEST_F (TrajectoryGeneratorCIRCTest, InterimPointJointGoal) | |
test the circ planner with interim point with joint goal | |
TEST_F (TrajectoryGeneratorCIRCTest, InterimPointJointGoalStartVelNearZero) | |
test the circ planner with interim point with joint goal and a close to zero velocity of the start state | |
TEST_F (TrajectoryGeneratorCIRCTest, InterimPointPoseGoal) | |
test the circ planner with interim point with pose goal | |
int | main (int argc, char **argv) |
int main | ( | int | argc, |
char ** | argv | ||
) |
Definition at line 760 of file unittest_trajectory_generator_circ.cpp.
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 | ( | 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 | ( | 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 | ( | 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 | ( | TrajectoryGeneratorCIRCTest | , |
CenterPointPoseGoalFrameIdOrientationConstraints | |||
) |
Set a frame id only on the orientation constraints.
Definition at line 678 of file unittest_trajectory_generator_circ.cpp.
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 | ( | 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 | ( | 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 | ( | 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 | ( | 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 | ( | 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 | ( | 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 | ( | 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 | ( | 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 | ( | 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 | ( | 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 | ( | 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 | ( | 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 | ( | 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 | ( | TrajectoryGeneratorCIRCTest | , |
invalidCenter | |||
) |
test the circ planner with invalid center point
Definition at line 443 of file unittest_trajectory_generator_circ.cpp.
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 | ( | 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 | ( | 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 | ( | 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 | ( | 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 | ( | TrajectoryGeneratorCIRCTest | , |
ValidCommand | |||
) |
Definition at line 311 of file unittest_trajectory_generator_circ.cpp.
TEST_F | ( | TrajectoryGeneratorCIRCTest | , |
velScaleToHigh | |||
) |
Generate invalid circ with to high vel scaling.
Definition at line 323 of file unittest_trajectory_generator_circ.cpp.