moveit2
The MoveIt Motion Planning Framework for ROS 2.
Classes | Namespaces | Macros | Functions
test_utils.h File Reference
#include <gtest/gtest.h>
#include <moveit_msgs/msg/motion_sequence_request.hpp>
#include <pilz_industrial_motion_planner/limits_container.h>
#include <pilz_industrial_motion_planner/trajectory_blend_request.h>
#include <pilz_industrial_motion_planner/trajectory_blend_response.h>
#include <pilz_industrial_motion_planner/trajectory_generator.h>
#include <boost/core/demangle.hpp>
#include <math.h>
#include <moveit/kinematic_constraints/utils.h>
#include <moveit/planning_interface/planning_interface.h>
#include <moveit/robot_trajectory/robot_trajectory.h>
#include <moveit_msgs/msg/constraints.hpp>
#include <moveit_msgs/action/move_group.hpp>
#include <sensor_msgs/msg/joint_state.hpp>
#include <string>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/convert.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <utility>
Include dependency graph for test_utils.h:
This graph shows which files directly or indirectly include this file:

Go to the source code of this file.

Classes

struct  testutils::BlendTestData
 Test data for blending, which contains three joint position vectors of three robot state. More...
 

Namespaces

 testutils
 

Macros

#define INSTANTIATE_TEST_SUITE_P(...)   INSTANTIATE_TEST_CASE_P(__VA_ARGS__)
 
#define TYPED_TEST_SUITE(...)   TYPED_TEST_CASE(__VA_ARGS__)
 

Functions

const std::string testutils::JOINT_NAME_PREFIX ("prbt_joint_")
 
std::string testutils::getJointName (size_t joint_number, const std::string &joint_prefix)
 
pilz_industrial_motion_planner::JointLimitsContainer testutils::createFakeLimits (const std::vector< std::string > &joint_names)
 Create limits for tests to avoid the need to get the limits from the node parameter. More...
 
std::string testutils::demangle (char const *name)
 
sensor_msgs::msg::JointState testutils::generateJointState (const std::vector< double > &pos, const std::vector< double > &vel, const std::string &joint_prefix=testutils::JOINT_NAME_PREFIX)
 
sensor_msgs::msg::JointState testutils::generateJointState (const std::vector< double > &pos, const std::string &joint_prefix=testutils::JOINT_NAME_PREFIX)
 
moveit_msgs::msg::Constraints testutils::generateJointConstraint (const std::vector< double > &pos_list, const std::string &joint_prefix=testutils::JOINT_NAME_PREFIX)
 
bool testutils::getExpectedGoalPose (const moveit::core::RobotModelConstPtr &robot_model, const planning_interface::MotionPlanRequest &req, std::string &link_name, Eigen::Isometry3d &goal_pose_expect)
 Determines the goal position from the given request. TRUE if successful, FALSE otherwise. More...
 
void testutils::createDummyRequest (const moveit::core::RobotModelConstPtr &robot_model, const std::string &planning_group, planning_interface::MotionPlanRequest &req)
 create a dummy motion plan request with zero start state No goal constraint is given. More...
 
void testutils::createPTPRequest (const std::string &planning_group, const moveit::core::RobotState &start_state, const moveit::core::RobotState &goal_state, planning_interface::MotionPlanRequest &req)
 
bool testutils::isGoalReached (const trajectory_msgs::msg::JointTrajectory &trajectory, const std::vector< moveit_msgs::msg::JointConstraint > &goal, const double joint_position_tolerance, const double joint_velocity_tolerance=1.0e-6)
 check if the goal given in joint space is reached Only the last point in the trajectory is veryfied. More...
 
bool testutils::isGoalReached (const moveit::core::RobotModelConstPtr &robot_model, const trajectory_msgs::msg::JointTrajectory &trajectory, const planning_interface::MotionPlanRequest &req, const double pos_tolerance, const double orientation_tolerance)
 check if the goal given in cartesian space is reached Only the last point in the trajectory is veryfied. More...
 
bool testutils::isGoalReached (const moveit::core::RobotModelConstPtr &robot_model, const trajectory_msgs::msg::JointTrajectory &trajectory, const planning_interface::MotionPlanRequest &req, const double tolerance)
 
bool testutils::checkCartesianLinearity (const moveit::core::RobotModelConstPtr &robot_model, const trajectory_msgs::msg::JointTrajectory &trajectory, const planning_interface::MotionPlanRequest &req, const double translation_norm_tolerance, const double rot_axis_norm_tolerance, const double rot_angle_tolerance=10e-5)
 Check that given trajectory is straight line. More...
 
bool testutils::checkSLERP (const Eigen::Isometry3d &start_pose, const Eigen::Isometry3d &goal_pose, const Eigen::Isometry3d &wp_pose, const double rot_axis_norm_tolerance, const double rot_angle_tolerance=10e-5)
 check SLERP. The orientation should rotate around the same axis linearly. More...
 
int testutils::getWayPointIndex (const robot_trajectory::RobotTrajectoryPtr &trajectory, const double time_from_start)
 get the waypoint index from time from start More...
 
bool testutils::checkJointTrajectory (const trajectory_msgs::msg::JointTrajectory &trajectory, const pilz_industrial_motion_planner::JointLimitsContainer &joint_limits)
 check joint trajectory of consistency, position, velocity and acceleration limits More...
 
::testing::AssertionResult testutils::hasStrictlyIncreasingTime (const robot_trajectory::RobotTrajectoryPtr &trajectory)
 Checks that every waypoint in the trajectory has a non-zero duration between itself and its predecessor. More...
 
bool testutils::isTrajectoryConsistent (const trajectory_msgs::msg::JointTrajectory &trajectory)
 check if the sizes of the joint position/veloicty/acceleration are correct More...
 
bool testutils::isPositionBounded (const trajectory_msgs::msg::JointTrajectory &trajectory, const pilz_industrial_motion_planner::JointLimitsContainer &joint_limits)
 is Position Bounded More...
 
bool testutils::isVelocityBounded (const trajectory_msgs::msg::JointTrajectory &trajectory, const pilz_industrial_motion_planner::JointLimitsContainer &joint_limits)
 is Velocity Bounded More...
 
bool testutils::isAccelerationBounded (const trajectory_msgs::msg::JointTrajectory &trajectory, const pilz_industrial_motion_planner::JointLimitsContainer &joint_limits)
 is Acceleration Bounded More...
 
bool testutils::toTCPPose (const moveit::core::RobotModelConstPtr &robot_model, const std::string &link_name, const std::vector< double > &joint_values, geometry_msgs::msg::Pose &pose, const std::string &joint_prefix=testutils::JOINT_NAME_PREFIX)
 compute the tcp pose from joint values More...
 
bool testutils::computeLinkFK (const moveit::core::RobotModelConstPtr &robot_model, const std::string &link_name, const std::map< std::string, double > &joint_state, Eigen::Isometry3d &pose)
 computeLinkFK More...
 
bool testutils::checkOriginalTrajectoryAfterBlending (const pilz_industrial_motion_planner::TrajectoryBlendRequest &req, const pilz_industrial_motion_planner::TrajectoryBlendResponse &res, const double time_tolerance)
 checkOriginalTrajectoryAfterBlending More...
 
bool testutils::checkBlendingJointSpaceContinuity (const pilz_industrial_motion_planner::TrajectoryBlendResponse &res, double joint_velocity_tolerance, double joint_accleration_tolerance)
 check the blending result, if the joint space continuity is fulfilled More...
 
bool testutils::checkBlendingCartSpaceContinuity (const pilz_industrial_motion_planner::TrajectoryBlendRequest &req, const pilz_industrial_motion_planner::TrajectoryBlendResponse &res, const pilz_industrial_motion_planner::LimitsContainer &planner_limits)
 
bool testutils::checkThatPointsInRadius (const std::string &link_name, const double &r, Eigen::Isometry3d &circ_pose, const pilz_industrial_motion_planner::TrajectoryBlendResponse &res)
 Checks if all points of the blending trajectory lie within the blending radius. More...
 
void testutils::computeCartVelocity (const Eigen::Isometry3d &pose_1, const Eigen::Isometry3d &pose_2, double duration, Eigen::Vector3d &v, Eigen::Vector3d &w)
 compute Cartesian velocity More...
 
void testutils::getLinLinPosesWithoutOriChange (const std::string &frame_id, sensor_msgs::msg::JointState &initialJointState, geometry_msgs::msg::PoseStamped &p1, geometry_msgs::msg::PoseStamped &p2)
 Returns an initial joint state and two poses which can be used to perform a Lin-Lin movement. More...
 
void testutils::getOriChange (Eigen::Matrix3d &ori1, Eigen::Matrix3d &ori2)
 
void testutils::createFakeCartTraj (const robot_trajectory::RobotTrajectoryPtr &traj, const std::string &link_name, moveit_msgs::msg::RobotTrajectory &fake_traj)
 
geometry_msgs::msg::Quaternion testutils::fromEuler (double a, double b, double c)
 
bool testutils::getBlendTestData (const rclcpp::Node::SharedPtr &node, const size_t &dataset_num, const std::string &name_prefix, std::vector< BlendTestData > &datasets)
 fetch test datasets from node parameters More...
 
bool testutils::checkBlendResult (const pilz_industrial_motion_planner::TrajectoryBlendRequest &blend_req, const pilz_industrial_motion_planner::TrajectoryBlendResponse &blend_res, const pilz_industrial_motion_planner::LimitsContainer &limits, double joint_velocity_tolerance, double joint_acceleration_tolerance, double cartesian_velocity_tolerance, double cartesian_angular_velocity_tolerance)
 check the blending result of lin-lin More...
 
bool testutils::generateTrajFromBlendTestData (const planning_scene::PlanningSceneConstPtr &scene, const std::shared_ptr< pilz_industrial_motion_planner::TrajectoryGenerator > &tg, const std::string &group_name, const std::string &link_name, const BlendTestData &data, const double &sampling_time_1, const double &sampling_time_2, planning_interface::MotionPlanResponse &res_lin_1, planning_interface::MotionPlanResponse &res_lin_2, double &dis_lin_1, double &dis_lin_2)
 generate two LIN trajectories from test data set More...
 
void testutils::generateRequestMsgFromBlendTestData (const moveit::core::RobotModelConstPtr &robot_model, const BlendTestData &data, const std::string &planner_id, const std::string &group_name, const std::string &link_name, moveit_msgs::msg::MotionSequenceRequest &req_list)
 
void testutils::checkRobotModel (const moveit::core::RobotModelConstPtr &robot_model, const std::string &group_name, const std::string &link_name)
 
::testing::AssertionResult testutils::hasTrapezoidVelocity (std::vector< double > accelerations, const double acc_tol)
 Check that a given vector of accelerations represents a trapezoid velocity profile. More...
 
::testing::AssertionResult testutils::checkCartesianTranslationalPath (const robot_trajectory::RobotTrajectoryConstPtr &trajectory, const std::string &link_name, const double acc_tol=DEFAULT_ACCELERATION_EQUALITY_TOLERANCE)
 Check that the translational path of a given trajectory has a trapezoid velocity profile. More...
 
::testing::AssertionResult testutils::checkCartesianRotationalPath (const robot_trajectory::RobotTrajectoryConstPtr &trajectory, const std::string &link_name, const double rot_axis_tol=DEFAULT_ROTATION_AXIS_EQUALITY_TOLERANCE, const double acc_tol=DEFAULT_ACCELERATION_EQUALITY_TOLERANCE)
 Check that the rotational path of a given trajectory is a quaternion slerp. More...
 
bool testutils::isMonotonouslyDecreasing (const std::vector< double > &vec, const double &tol)
 

Macro Definition Documentation

◆ INSTANTIATE_TEST_SUITE_P

#define INSTANTIATE_TEST_SUITE_P (   ...)    INSTANTIATE_TEST_CASE_P(__VA_ARGS__)

Definition at line 39 of file test_utils.h.

◆ TYPED_TEST_SUITE

#define TYPED_TEST_SUITE (   ...)    TYPED_TEST_CASE(__VA_ARGS__)

Definition at line 42 of file test_utils.h.