moveit2
The MoveIt Motion Planning Framework for ROS 2.
test_utils.h
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2018 Pilz GmbH & Co. KG
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Pilz GmbH & Co. KG nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 #pragma once
36 
37 #include <gtest/gtest.h>
38 #ifndef INSTANTIATE_TEST_SUITE_P // prior to gtest 1.10
39 #define INSTANTIATE_TEST_SUITE_P(...) INSTANTIATE_TEST_CASE_P(__VA_ARGS__)
40 #endif
41 #ifndef TYPED_TEST_SUITE // prior to gtest 1.10
42 #define TYPED_TEST_SUITE(...) TYPED_TEST_CASE(__VA_ARGS__)
43 #endif
44 
45 #include <moveit_msgs/msg/motion_sequence_request.hpp>
50 #include <boost/core/demangle.hpp>
51 #include <math.h>
55 #include <moveit_msgs/msg/constraints.hpp>
56 #include <moveit_msgs/action/move_group.hpp>
57 #include <sensor_msgs/msg/joint_state.hpp>
58 #include <string>
59 #include <tf2/LinearMath/Quaternion.h>
60 #include <tf2/convert.h>
61 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
62 #include <utility>
63 
64 namespace testutils
65 {
66 const std::string JOINT_NAME_PREFIX("prbt_joint_");
67 
68 static constexpr int32_t DEFAULT_SERVICE_TIMEOUT(10);
69 static constexpr double DEFAULT_ACCELERATION_EQUALITY_TOLERANCE{ 1e-6 };
70 static constexpr double DEFAULT_ROTATION_AXIS_EQUALITY_TOLERANCE{ 1e-8 };
71 
75 inline static constexpr double deg2Rad(double angle)
76 {
77  return (angle / 180.0) * M_PI;
78 }
79 
80 inline std::string getJointName(size_t joint_number, const std::string& joint_prefix)
81 {
82  return joint_prefix + std::to_string(joint_number);
83 }
84 
89 pilz_industrial_motion_planner::JointLimitsContainer createFakeLimits(const std::vector<std::string>& joint_names);
90 
91 inline std::string demangle(char const* name)
92 {
94 }
95 
96 //********************************************
97 // Motion plan requests
98 //********************************************
99 
100 inline sensor_msgs::msg::JointState generateJointState(const std::vector<double>& pos, const std::vector<double>& vel,
101  const std::string& joint_prefix = testutils::JOINT_NAME_PREFIX)
102 {
103  sensor_msgs::msg::JointState state;
104  auto posit = pos.cbegin();
105  size_t i = 0;
106 
107  while (posit != pos.cend())
108  {
109  state.name.push_back(testutils::getJointName(i + 1, joint_prefix));
110  state.position.push_back(*posit);
111 
112  i++;
113  posit++;
114  }
115  for (const auto& one_vel : vel)
116  {
117  state.velocity.push_back(one_vel);
118  }
119  return state;
120 }
121 
122 inline sensor_msgs::msg::JointState generateJointState(const std::vector<double>& pos,
123  const std::string& joint_prefix = testutils::JOINT_NAME_PREFIX)
124 {
125  return generateJointState(pos, std::vector<double>(), joint_prefix);
126 }
127 
128 inline moveit_msgs::msg::Constraints
129 generateJointConstraint(const std::vector<double>& pos_list,
130  const std::string& joint_prefix = testutils::JOINT_NAME_PREFIX)
131 {
132  moveit_msgs::msg::Constraints gc;
133 
134  auto pos_it = pos_list.begin();
135 
136  for (size_t i = 0; i < pos_list.size(); ++i)
137  {
138  moveit_msgs::msg::JointConstraint jc;
139  jc.joint_name = testutils::getJointName(i + 1, joint_prefix);
140  jc.position = *pos_it;
141  gc.joint_constraints.push_back(jc);
142 
143  ++pos_it;
144  }
145 
146  return gc;
147 }
148 
153 bool getExpectedGoalPose(const moveit::core::RobotModelConstPtr& robot_model,
154  const planning_interface::MotionPlanRequest& req, std::string& link_name,
155  Eigen::Isometry3d& goal_pose_expect);
156 
164 void createDummyRequest(const moveit::core::RobotModelConstPtr& robot_model, const std::string& planning_group,
166 
167 void createPTPRequest(const std::string& planning_group, const moveit::core::RobotState& start_state,
169 
179 bool isGoalReached(const trajectory_msgs::msg::JointTrajectory& trajectory,
180  const std::vector<moveit_msgs::msg::JointConstraint>& goal, const double joint_position_tolerance,
181  const double joint_velocity_tolerance = 1.0e-6);
182 
193 bool isGoalReached(const moveit::core::RobotModelConstPtr& robot_model,
194  const trajectory_msgs::msg::JointTrajectory& trajectory,
195  const planning_interface::MotionPlanRequest& req, const double pos_tolerance,
196  const double orientation_tolerance);
197 
198 bool isGoalReached(const moveit::core::RobotModelConstPtr& robot_model,
199  const trajectory_msgs::msg::JointTrajectory& trajectory,
200  const planning_interface::MotionPlanRequest& req, const double tolerance);
201 
205 bool checkCartesianLinearity(const moveit::core::RobotModelConstPtr& robot_model,
206  const trajectory_msgs::msg::JointTrajectory& trajectory,
207  const planning_interface::MotionPlanRequest& req, const double translation_norm_tolerance,
208  const double rot_axis_norm_tolerance, const double rot_angle_tolerance = 10e-5);
209 
219 bool checkSLERP(const Eigen::Isometry3d& start_pose, const Eigen::Isometry3d& goal_pose,
220  const Eigen::Isometry3d& wp_pose, const double rot_axis_norm_tolerance,
221  const double rot_angle_tolerance = 10e-5);
222 
229 inline int getWayPointIndex(const robot_trajectory::RobotTrajectoryPtr& trajectory, const double time_from_start)
230 {
231  int index_before, index_after;
232  double blend;
233  trajectory->findWayPointIndicesForDurationAfterStart(time_from_start, index_before, index_after, blend);
234  return blend > 0.5 ? index_after : index_before;
235 }
236 
244 bool checkJointTrajectory(const trajectory_msgs::msg::JointTrajectory& trajectory,
246 
256 ::testing::AssertionResult hasStrictlyIncreasingTime(const robot_trajectory::RobotTrajectoryPtr& trajectory);
257 
264 bool isTrajectoryConsistent(const trajectory_msgs::msg::JointTrajectory& trajectory);
265 
272 bool isPositionBounded(const trajectory_msgs::msg::JointTrajectory& trajectory,
274 
281 bool isVelocityBounded(const trajectory_msgs::msg::JointTrajectory& trajectory,
283 
290 bool isAccelerationBounded(const trajectory_msgs::msg::JointTrajectory& trajectory,
292 
302 bool toTCPPose(const moveit::core::RobotModelConstPtr& robot_model, const std::string& link_name,
303  const std::vector<double>& joint_values, geometry_msgs::msg::Pose& pose,
304  const std::string& joint_prefix = testutils::JOINT_NAME_PREFIX);
305 
314 bool computeLinkFK(const moveit::core::RobotModelConstPtr& robot_model, const std::string& link_name,
315  const std::map<std::string, double>& joint_state, Eigen::Isometry3d& pose);
316 
327  const double time_tolerance);
328 
337  double joint_velocity_tolerance, double joint_accleration_tolerance);
338 
342 
347 bool checkThatPointsInRadius(const std::string& link_name, const double& r, Eigen::Isometry3d& circ_pose,
349 
358 void computeCartVelocity(const Eigen::Isometry3d& pose_1, const Eigen::Isometry3d& pose_2, double duration,
360 
370 void getLinLinPosesWithoutOriChange(const std::string& frame_id, sensor_msgs::msg::JointState& initialJointState,
371  geometry_msgs::msg::PoseStamped& p1, geometry_msgs::msg::PoseStamped& p2);
372 
373 void getOriChange(Eigen::Matrix3d& ori1, Eigen::Matrix3d& ori2);
374 
375 void createFakeCartTraj(const robot_trajectory::RobotTrajectoryPtr& traj, const std::string& link_name,
376  moveit_msgs::msg::RobotTrajectory& fake_traj);
377 
378 inline geometry_msgs::msg::Quaternion fromEuler(double a, double b, double c)
379 {
380  tf2::Vector3 qvz(0., 0., 1.);
381  tf2::Vector3 qvy(0., 1., 0.);
382  tf2::Quaternion q1(qvz, a);
383 
384  q1 = q1 * tf2::Quaternion(qvy, b);
385  q1 = q1 * tf2::Quaternion(qvz, c);
386 
387  return tf2::toMsg(q1);
388 }
389 
395 {
396  std::vector<double> start_position;
397  std::vector<double> mid_position;
398  std::vector<double> end_position;
399 };
400 
406 bool getBlendTestData(const rclcpp::Node::SharedPtr& node, const size_t& dataset_num, const std::string& name_prefix,
407  std::vector<BlendTestData>& datasets);
408 
419  const pilz_industrial_motion_planner::LimitsContainer& limits, double joint_velocity_tolerance,
420  double joint_acceleration_tolerance, double cartesian_velocity_tolerance,
421  double cartesian_angular_velocity_tolerance);
422 
434 bool generateTrajFromBlendTestData(const planning_scene::PlanningSceneConstPtr& scene,
435  const std::shared_ptr<pilz_industrial_motion_planner::TrajectoryGenerator>& tg,
436  const std::string& group_name, const std::string& link_name,
437  const BlendTestData& data, const double& sampling_time_1,
438  const double& sampling_time_2, planning_interface::MotionPlanResponse& res_lin_1,
439  planning_interface::MotionPlanResponse& res_lin_2, double& dis_lin_1,
440  double& dis_lin_2);
441 
442 void generateRequestMsgFromBlendTestData(const moveit::core::RobotModelConstPtr& robot_model, const BlendTestData& data,
443  const std::string& planner_id, const std::string& group_name,
444  const std::string& link_name,
445  moveit_msgs::msg::MotionSequenceRequest& req_list);
446 
447 void checkRobotModel(const moveit::core::RobotModelConstPtr& robot_model, const std::string& group_name,
448  const std::string& link_name);
449 
454 ::testing::AssertionResult hasTrapezoidVelocity(std::vector<double> accelerations, const double acc_tol);
455 
461 ::testing::AssertionResult
462 checkCartesianTranslationalPath(const robot_trajectory::RobotTrajectoryConstPtr& trajectory,
463  const std::string& link_name,
464  const double acc_tol = DEFAULT_ACCELERATION_EQUALITY_TOLERANCE);
465 
472 ::testing::AssertionResult
473 checkCartesianRotationalPath(const robot_trajectory::RobotTrajectoryConstPtr& trajectory, const std::string& link_name,
474  const double rot_axis_tol = DEFAULT_ROTATION_AXIS_EQUALITY_TOLERANCE,
475  const double acc_tol = DEFAULT_ACCELERATION_EQUALITY_TOLERANCE);
476 
477 inline bool isMonotonouslyDecreasing(const std::vector<double>& vec, const double& tol)
478 {
479  return std::is_sorted(vec.begin(), vec.end(), [tol](double a, double b) {
480  return !(std::abs(a - b) < tol || a < b); // true -> a is ordered before b -> list is not sorted
481  });
482 }
483 
484 } // namespace testutils
Representation of a robot's state. This includes position, velocity, acceleration and effort.
Definition: robot_state.h:90
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.
Vec3fX< details::Vec3Data< double > > Vector3d
Definition: fcl_compat.h:89
frame_id
Definition: pick.py:63
w
Definition: pick.py:67
scene
Definition: pick.py:52
a
Definition: plan.py:54
r
Definition: plan.py:56
moveit_msgs::msg::MotionPlanRequest MotionPlanRequest
name
Definition: setup.py:7
bool isMonotonouslyDecreasing(const std::vector< double > &vec, const double &tol)
Definition: test_utils.h:477
bool checkBlendingCartSpaceContinuity(const pilz_industrial_motion_planner::TrajectoryBlendRequest &req, const pilz_industrial_motion_planner::TrajectoryBlendResponse &res, const pilz_industrial_motion_planner::LimitsContainer &planner_limits)
Definition: test_utils.cpp:775
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.
Definition: test_utils.cpp:118
bool computeLinkFK(const moveit::core::RobotModelConstPtr &robot_model, const std::string &link_name, const std::map< std::string, double > &joint_state, Eigen::Isometry3d &pose)
computeLinkFK
Definition: test_utils.cpp:321
sensor_msgs::msg::JointState generateJointState(const std::vector< double > &pos, const std::vector< double > &vel, const std::string &joint_prefix=testutils::JOINT_NAME_PREFIX)
Definition: test_utils.h:100
::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 isVelocityBounded(const trajectory_msgs::msg::JointTrajectory &trajectory, const pilz_industrial_motion_planner::JointLimitsContainer &joint_limits)
is Velocity Bounded
Definition: test_utils.cpp:353
void 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.
Definition: test_utils.cpp:504
int getWayPointIndex(const robot_trajectory::RobotTrajectoryPtr &trajectory, const double time_from_start)
get the waypoint index from time from start
Definition: test_utils.h:229
bool 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.
Definition: test_utils.cpp:71
bool isAccelerationBounded(const trajectory_msgs::msg::JointTrajectory &trajectory, const pilz_industrial_motion_planner::JointLimitsContainer &joint_limits)
is Acceleration Bounded
Definition: test_utils.cpp:393
bool 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.
Definition: test_utils.cpp:929
void 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)
bool 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
bool isTrajectoryConsistent(const trajectory_msgs::msg::JointTrajectory &trajectory)
check if the sizes of the joint position/veloicty/acceleration are correct
Definition: test_utils.cpp:376
void createFakeCartTraj(const robot_trajectory::RobotTrajectoryPtr &traj, const std::string &link_name, moveit_msgs::msg::RobotTrajectory &fake_traj)
Definition: test_utils.cpp:990
moveit_msgs::msg::Constraints generateJointConstraint(const std::vector< double > &pos_list, const std::string &joint_prefix=testutils::JOINT_NAME_PREFIX)
Definition: test_utils.h:129
bool 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
bool 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
pilz_industrial_motion_planner::JointLimitsContainer createFakeLimits(const std::vector< std::string > &joint_names)
Create limits for tests to avoid the need to get the limits from the node parameter.
Definition: test_utils.cpp:48
bool 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.
Definition: test_utils.cpp:292
::testing::AssertionResult hasStrictlyIncreasingTime(const robot_trajectory::RobotTrajectoryPtr &trajectory)
Checks that every waypoint in the trajectory has a non-zero duration between itself and its predecess...
Definition: test_utils.cpp:487
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
Definition: test_utils.cpp:460
std::string getJointName(size_t joint_number, const std::string &joint_prefix)
Definition: test_utils.h:80
void computeCartVelocity(const Eigen::Isometry3d &pose_1, const Eigen::Isometry3d &pose_2, double duration, Eigen::Vector3d &v, Eigen::Vector3d &w)
compute Cartesian velocity
Definition: test_utils.cpp:946
geometry_msgs::msg::Quaternion fromEuler(double a, double b, double c)
Definition: test_utils.h:378
void getOriChange(Eigen::Matrix3d &ori1, Eigen::Matrix3d &ori2)
Definition: test_utils.cpp:984
bool 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
Definition: test_utils.cpp:531
std::string demangle(char const *name)
Definition: test_utils.h:91
void 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.
Definition: test_utils.cpp:964
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.
Definition: test_utils.cpp:222
const std::string JOINT_NAME_PREFIX("prbt_joint_")
bool isPositionBounded(const trajectory_msgs::msg::JointTrajectory &trajectory, const pilz_industrial_motion_planner::JointLimitsContainer &joint_limits)
is Position Bounded
Definition: test_utils.cpp:435
::testing::AssertionResult hasTrapezoidVelocity(std::vector< double > accelerations, const double acc_tol)
Check that a given vector of accelerations represents a trapezoid velocity profile.
bool checkOriginalTrajectoryAfterBlending(const pilz_industrial_motion_planner::TrajectoryBlendRequest &req, const pilz_industrial_motion_planner::TrajectoryBlendResponse &res, const double time_tolerance)
checkOriginalTrajectoryAfterBlending
Definition: test_utils.cpp:561
bool 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
Definition: test_utils.cpp:671
void createPTPRequest(const std::string &planning_group, const moveit::core::RobotState &start_state, const moveit::core::RobotState &goal_state, planning_interface::MotionPlanRequest &req)
Definition: test_utils.cpp:516
void checkRobotModel(const moveit::core::RobotModelConstPtr &robot_model, const std::string &group_name, const std::string &link_name)
::testing::AssertionResult 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.
Test data for blending, which contains three joint position vectors of three robot state.
Definition: test_utils.h:395
std::vector< double > end_position
Definition: test_utils.h:398
std::vector< double > start_position
Definition: test_utils.h:396
std::vector< double > mid_position
Definition: test_utils.h:397