37#include <gtest/gtest.h>
51#include <rclcpp/rclcpp.hpp>
86 rclcpp::NodeOptions node_options;
87 node_options.automatically_declare_parameters_from_overrides(
true);
88 node_ = rclcpp::Node::make_shared(
"unittest_trajectory_generator_lin", node_options);
91 rm_loader_ = std::make_unique<robot_model_loader::RobotModelLoader>(
node_);
93 ASSERT_TRUE(
bool(
robot_model_)) <<
"Failed to load robot model";
112 ASSERT_NE(
nullptr,
tdp_) <<
"Failed to load test data by provider.";
122 cartesian_limits::Params cart_limits;
123 cart_limits.max_rot_vel = 0.5 * M_PI;
124 cart_limits.max_trans_acc = 2;
125 cart_limits.max_trans_dec = 2;
126 cart_limits.max_trans_vel = 1;
133 ASSERT_NE(
nullptr,
lin_) <<
"Failed to create LIN trajectory generator.";
144 moveit_msgs::msg::MotionPlanResponse res_msg;
169 std::unique_ptr<robot_model_loader::RobotModelLoader>
rm_loader_;
173 std::unique_ptr<TrajectoryGenerator>
lin_;
175 std::unique_ptr<pilz_industrial_motion_planner_testutils::TestdataLoader>
tdp_;
192 auto ltcf_ex = std::make_shared<LinTrajectoryConversionFailure>(
"");
193 EXPECT_EQ(ltcf_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::FAILURE);
197 auto jnm_ex = std::make_shared<JointNumberMismatch>(
"");
198 EXPECT_EQ(jnm_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS);
202 auto ljmiss_ex = std::make_shared<LinJointMissingInStartState>(
"");
203 EXPECT_EQ(ljmiss_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE);
207 auto lifgi_ex = std::make_shared<LinInverseForGoalIncalculable>(
"");
208 EXPECT_EQ(lifgi_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION);
221 req.start_state.joint_state.velocity.push_back(1.0);
225 lin_->generate(planning_scene_, req, res);
226 EXPECT_EQ(res.
error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE);
238 lin_->generate(planning_scene_, lin_joint_req, res);
239 EXPECT_TRUE(res.
error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
242 EXPECT_TRUE(checkLinResponse(lin_joint_req, res));
254 lin_joint_req.start_state.joint_state.velocity =
255 std::vector<double>(lin_joint_req.start_state.joint_state.position.size(), 1e-16);
259 lin_->generate(planning_scene_, lin_joint_req, res);
260 EXPECT_TRUE(res.
error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
263 EXPECT_TRUE(checkLinResponse(lin_joint_req, res));
272 moveit_msgs::msg::MotionPlanRequest lin_cart_req{ tdp_->getLinCart(
"lin2").toRequest() };
276 lin_->generate(planning_scene_, lin_cart_req, res);
277 EXPECT_TRUE(res.
error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
280 EXPECT_TRUE(checkLinResponse(lin_cart_req, res));
294 moveit_msgs::msg::MotionPlanRequest lin_joint_req{ tdp_->getLinJoint(
"lin2").toRequest() };
300 lin_->generate(planning_scene_, lin_joint_req, res, 0.01);
301 EXPECT_EQ(res.
error_code.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
306 for (
size_t idx = 0; idx < res.
trajectory->getLastWayPointPtr()->getVariableCount(); ++idx)
308 EXPECT_NEAR(0.0, res.
trajectory->getLastWayPointPtr()->getVariableVelocity(idx), other_tolerance_);
309 EXPECT_NEAR(0.0, res.
trajectory->getLastWayPointPtr()->getVariableAcceleration(idx), other_tolerance_);
328 LinJoint lin{ tdp_->getLinJoint(
"lin2") };
332 lin_->generate(planning_scene_, lin.toRequest(), res);
333 ASSERT_FALSE(res.
error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
349 LinJoint lin{ tdp_->getLinJoint(
"lin2") };
353 lin.getGoalConfiguration().setJoint(2, 0.96);
354 lin.getGoalConfiguration().setJoint(4, -2.48);
355 lin.setVelocityScale(1.0);
356 lin.setAccelerationScale(1.0);
359 lin_->generate(planning_scene_, lin.toRequest(), res);
360 ASSERT_FALSE(res.
error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
375 moveit_msgs::msg::MotionPlanRequest lin_joint_req{ tdp_->getLinJoint(
"lin2").toRequest() };
378 jointStateToRobotState(lin_joint_req.start_state.joint_state, start_state);
380 for (
auto& joint_constraint : lin_joint_req.goal_constraints.at(0).joint_constraints)
387 lin_->generate(planning_scene_, lin_joint_req, res);
388 EXPECT_TRUE(res.
error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
391 EXPECT_TRUE(checkLinResponse(lin_joint_req, res));
407 moveit_msgs::msg::MotionPlanRequest lin_joint_req{ tdp_->getLinJoint(
"lin2").toRequest() };
410 lin_joint_req.goal_constraints.front().joint_constraints.pop_back();
414 lin_->generate(planning_scene_, lin_joint_req, res);
415 EXPECT_TRUE(res.
error_code.val == moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS);
425 moveit_msgs::msg::MotionPlanRequest lin_cart_req{ tdp_->getLinCart(
"lin2").toRequest() };
426 EXPECT_GT(lin_cart_req.start_state.joint_state.name.size(), 1u);
427 lin_cart_req.start_state.joint_state.name.resize(1);
428 lin_cart_req.start_state.joint_state.position.resize(1);
432 lin_->generate(planning_scene_, lin_cart_req, res);
433 EXPECT_EQ(res.
error_code.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE);
443 moveit_msgs::msg::MotionPlanRequest lin_cart_req{ tdp_->getLinCart(
"lin2").toRequest() };
445 lin_cart_req.goal_constraints.front().position_constraints.front().header.frame_id = robot_model_->getModelFrame();
446 lin_cart_req.goal_constraints.front().orientation_constraints.front().header.frame_id = robot_model_->getModelFrame();
450 lin_->generate(planning_scene_, lin_cart_req, res);
451 EXPECT_TRUE(res.
error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
454 EXPECT_TRUE(checkLinResponse(lin_cart_req, res));
459 rclcpp::init(argc, argv);
460 testing::InitGoogleTest(&argc, argv);
461 return RUN_ALL_TESTS();
Parameterized unittest of trajectory generator LIN to enable tests against different robot models....
double joint_position_tolerance_
bool checkLinResponse(const planning_interface::MotionPlanRequest &req, const planning_interface::MotionPlanResponse &res)
std::string test_data_file_name_
rclcpp::Node::SharedPtr node_
LimitsContainer planner_limits_
double velocity_scaling_factor_
double pose_norm_tolerance_
moveit::core::RobotModelConstPtr robot_model_
std::unique_ptr< pilz_industrial_motion_planner_testutils::TestdataLoader > tdp_
void SetUp() override
Create test scenario for lin trajectory generator.
std::string target_link_hcd_
std::string planning_group_
std::unique_ptr< TrajectoryGenerator > lin_
planning_scene::PlanningSceneConstPtr planning_scene_
double joint_velocity_tolerance_
double rot_axis_norm_tolerance_
std::unique_ptr< robot_model_loader::RobotModelLoader > rm_loader_
Representation of a robot's state. This includes position, velocity, acceleration and effort.
double getVariablePosition(const std::string &variable) const
Get the position of a particular variable. An exception is thrown if the variable is not known.
static JointLimitsContainer getAggregatedLimits(const rclcpp::Node::SharedPtr &node, const std::string ¶m_namespace, const std::vector< const moveit::core::JointModel * > &joint_models)
Aggregates(combines) the joint limits from joint model and node parameters. The rules for the combina...
Container for JointLimits, essentially a map with convenience functions. Adds the ability to as for l...
This class combines CartesianLimit and JointLimits into on single class.
void setCartesianLimits(cartesian_limits::Params &cartesian_limit)
Set cartesian limits.
void setJointLimits(JointLimitsContainer &joint_limits)
Set joint limits.
const JointLimitsContainer & getJointLimitContainer() const
Obtain the Joint Limits from the container.
GoalType & getGoalConfiguration()
Data class storing all information regarding a linear command.
void setAccelerationScale(double acceleration_scale)
TEST_F(GetSolverTipFrameIntegrationTest, TestHasSolverManipulator)
Check if hasSolver() can be called successfully for the manipulator group.
moveit_msgs::msg::MotionPlanRequest MotionPlanRequest
bool 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 verified.
::testing::AssertionResult 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.
bool 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
bool 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.
void checkRobotModel(const moveit::core::RobotModelConstPtr &robot_model, const std::string &group_name, const std::string &link_name)
Response to a planning query.
moveit::core::MoveItErrorCode error_code
robot_trajectory::RobotTrajectoryPtr trajectory
void getMessage(moveit_msgs::msg::MotionPlanResponse &msg) const
Construct a ROS message from struct data.
const std::string PARAM_NAMESPACE_LIMITS
const std::string PARAM_PLANNING_GROUP_NAME("planning_group")
const std::string RANDOM_TEST_TRIAL_NUM("random_trial_number")
const std::string ROTATION_AXIS_NORM_TOLERANCE("rot_axis_norm_tolerance")
const std::string JOINT_POSITION_TOLERANCE("joint_position_tolerance")
int main(int argc, char **argv)
const std::string TARGET_LINK_HCD("target_link_hand_computed_data")
const std::string VELOCITY_SCALING_FACTOR("velocity_scaling_factor")
const std::string POSE_TRANSFORM_MATRIX_NORM_TOLERANCE("pose_norm_tolerance")
const std::string TEST_DATA_FILE_NAME("testdata_file_name")
const std::string OTHER_TOLERANCE("other_tolerance")
const std::string JOINT_VELOCITY_TOLERANCE("joint_velocity_tolerance")