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_);
92 robot_model_ = rm_loader_->getModel();
93 ASSERT_TRUE(
bool(robot_model_)) <<
"Failed to load robot model";
94 planning_scene_ = std::make_shared<planning_scene::PlanningScene>(robot_model_);
111 tdp_ = std::make_unique<pilz_industrial_motion_planner_testutils::XmlTestdataLoader>(test_data_file_name_);
112 ASSERT_NE(
nullptr, tdp_) <<
"Failed to load test data by provider.";
114 tdp_->setRobotModel(robot_model_);
127 planner_limits_.setCartesianLimits(cart_limits);
130 lin_ = std::make_unique<TrajectoryGeneratorLIN>(robot_model_, planner_limits_, planning_group_);
131 ASSERT_NE(
nullptr, lin_) <<
"Failed to create LIN trajectory generator.";
136 robot_model_.reset();
142 moveit_msgs::msg::MotionPlanResponse res_msg;
150 pose_norm_tolerance_, rot_axis_norm_tolerance_))
167 std::unique_ptr<robot_model_loader::RobotModelLoader>
rm_loader_;
171 std::unique_ptr<TrajectoryGenerator>
lin_;
173 std::unique_ptr<pilz_industrial_motion_planner_testutils::TestdataLoader>
tdp_;
190 std::shared_ptr<LinTrajectoryConversionFailure> ltcf_ex{
new LinTrajectoryConversionFailure(
"") };
191 EXPECT_EQ(ltcf_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::FAILURE);
195 std::shared_ptr<JointNumberMismatch> jnm_ex{
new JointNumberMismatch(
"") };
196 EXPECT_EQ(jnm_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS);
200 std::shared_ptr<LinJointMissingInStartState> ljmiss_ex{
new LinJointMissingInStartState(
"") };
201 EXPECT_EQ(ljmiss_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE);
205 std::shared_ptr<LinInverseForGoalIncalculable> lifgi_ex{
new LinInverseForGoalIncalculable(
"") };
206 EXPECT_EQ(lifgi_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION);
219 req.start_state.joint_state.velocity.push_back(1.0);
223 EXPECT_FALSE(lin_->generate(planning_scene_, req, res));
224 EXPECT_EQ(res.
error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE);
236 ASSERT_TRUE(lin_->generate(planning_scene_, lin_joint_req, res));
237 EXPECT_TRUE(res.
error_code_.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
240 EXPECT_TRUE(checkLinResponse(lin_joint_req, res));
252 lin_joint_req.start_state.joint_state.velocity =
253 std::vector<double>(lin_joint_req.start_state.joint_state.position.size(), 1e-16);
257 ASSERT_TRUE(lin_->generate(planning_scene_, lin_joint_req, res));
258 EXPECT_TRUE(res.
error_code_.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
261 EXPECT_TRUE(checkLinResponse(lin_joint_req, res));
274 ASSERT_TRUE(lin_->generate(planning_scene_, lin_cart_req, res));
275 EXPECT_TRUE(res.
error_code_.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
278 EXPECT_TRUE(checkLinResponse(lin_cart_req, res));
298 ASSERT_TRUE(lin_->generate(planning_scene_, lin_joint_req, res, 0.01));
299 EXPECT_EQ(res.
error_code_.val, moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
304 for (
size_t idx = 0; idx < res.
trajectory_->getLastWayPointPtr()->getVariableCount(); ++idx)
306 EXPECT_NEAR(0.0, res.
trajectory_->getLastWayPointPtr()->getVariableVelocity(idx), other_tolerance_);
307 EXPECT_NEAR(0.0, res.
trajectory_->getLastWayPointPtr()->getVariableAcceleration(idx), other_tolerance_);
326 LinJoint lin{ tdp_->getLinJoint(
"lin2") };
330 ASSERT_FALSE(lin_->generate(planning_scene_, lin.toRequest(), res));
346 LinJoint lin{ tdp_->getLinJoint(
"lin2") };
350 lin.getGoalConfiguration().setJoint(2, 0.96);
351 lin.getGoalConfiguration().setJoint(4, -2.48);
352 lin.setVelocityScale(1.0);
353 lin.setAccelerationScale(1.0);
356 ASSERT_FALSE(lin_->generate(planning_scene_, lin.toRequest(), res));
376 for (
auto& joint_constraint : lin_joint_req.goal_constraints.at(0).joint_constraints)
383 ASSERT_TRUE(lin_->generate(planning_scene_, lin_joint_req, res));
384 EXPECT_TRUE(res.
error_code_.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
387 EXPECT_TRUE(checkLinResponse(lin_joint_req, res));
404 pilz_industrial_motion_planner::TrajectoryGeneratorInvalidLimitsException);
423 lin_joint_req.goal_constraints.front().joint_constraints.pop_back();
427 ASSERT_FALSE(lin_->generate(planning_scene_, lin_joint_req, res));
428 EXPECT_TRUE(res.
error_code_.val == moveit_msgs::msg::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS);
439 EXPECT_GT(lin_cart_req.start_state.joint_state.name.size(), 1u);
440 lin_cart_req.start_state.joint_state.name.resize(1);
441 lin_cart_req.start_state.joint_state.position.resize(1);
445 EXPECT_FALSE(lin_->generate(planning_scene_, lin_cart_req, res));
446 EXPECT_EQ(res.
error_code_.val, moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE);
458 lin_cart_req.goal_constraints.front().position_constraints.front().header.frame_id = robot_model_->getModelFrame();
459 lin_cart_req.goal_constraints.front().orientation_constraints.front().header.frame_id = robot_model_->getModelFrame();
463 ASSERT_TRUE(lin_->generate(planning_scene_, lin_cart_req, res));
464 EXPECT_TRUE(res.
error_code_.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
467 EXPECT_TRUE(checkLinResponse(lin_cart_req, res));
470 int main(
int argc,
char** argv)
472 rclcpp::init(argc, argv);
473 testing::InitGoogleTest(&argc, argv);
474 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)
rclcpp::Node::SharedPtr node_
LimitsContainer planner_limits_
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 planning_group_
std::unique_ptr< TrajectoryGenerator > lin_
planning_scene::PlanningSceneConstPtr planning_scene_
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.
Set of cartesian limits, has values for velocity, acceleration and deceleration of both the translati...
void setMaxRotationalVelocity(double max_rot_vel)
Set the maximum rotational velocity.
void setMaxTranslationalVelocity(double max_trans_vel)
Set the maximal translational velocity.
void setMaxTranslationalAcceleration(double max_trans_acc)
Set the maximum translational acceleration.
void setMaxTranslationalDeceleration(double max_trans_dec)
Set the maximum translational deceleration.
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.
This class implements a linear trajectory generator in Cartesian space. The Cartesian trajetory are b...
GoalType & getGoalConfiguration()
Data class storing all information regarding a linear command.
void setAccelerationScale(double acceleration_scale)
bool jointStateToRobotState(const sensor_msgs::msg::JointState &joint_state, RobotState &state)
Convert a joint state to a MoveIt robot state.
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 veryfied.
::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)
robot_trajectory::RobotTrajectoryPtr trajectory_
void getMessage(moveit_msgs::msg::MotionPlanResponse &msg) const
moveit_msgs::msg::MoveItErrorCodes error_code_
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")