test_utils.hpp

Go to the documentation of this file.

/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2018 Pilz GmbH & Co. KG
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Pilz GmbH & Co. KG nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/
 
#pragma once
 
#include <gtest/gtest.h>
#ifndef INSTANTIATE_TEST_SUITE_P  // prior to gtest 1.10
#define INSTANTIATE_TEST_SUITE_P(...) INSTANTIATE_TEST_CASE_P(__VA_ARGS__)
#endif
#ifndef TYPED_TEST_SUITE  // prior to gtest 1.10
#define TYPED_TEST_SUITE(...) TYPED_TEST_CASE(__VA_ARGS__)
#endif
 
#include <moveit_msgs/msg/motion_sequence_request.hpp>
#include <pilz_industrial_motion_planner/limits_container.hpp>
#include <pilz_industrial_motion_planner/trajectory_blend_request.hpp>
#include <pilz_industrial_motion_planner/trajectory_blend_response.hpp>
#include <pilz_industrial_motion_planner/trajectory_generator.hpp>
#include <boost/core/demangle.hpp>
#include <math.h>
#include <moveit/kinematic_constraints/utils.hpp>
#include <moveit/planning_interface/planning_interface.hpp>
#include <moveit/robot_trajectory/robot_trajectory.hpp>
#include <moveit_msgs/msg/constraints.hpp>
#include <moveit_msgs/action/move_group.hpp>
#include <sensor_msgs/msg/joint_state.hpp>
#include <string>
// TODO: Remove conditional include swhen released to all active distros.
#if __has_include(<tf2/LinearMath/Quaternion.hpp>)
#include <tf2/LinearMath/Quaternion.hpp>
#else
#include <tf2/LinearMath/Quaternion.h>
#endif
#if __has_include(<tf2/convert.hpp>)
#include <tf2/convert.hpp>
#else
#include <tf2/convert.h>
#endif
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <utility>
 
namespace testutils
{
const std::string JOINT_NAME_PREFIX("prbt_joint_");
 
static constexpr int32_t DEFAULT_SERVICE_TIMEOUT(10);
static constexpr double DEFAULT_ACCELERATION_EQUALITY_TOLERANCE{ 1e-6 };
static constexpr double DEFAULT_ROTATION_AXIS_EQUALITY_TOLERANCE{ 1e-8 };
 
static inline constexpr double deg2Rad(double angle)
{
  return (angle / 180.0) * M_PI;
}
 
inline std::string getJointName(size_t joint_number, const std::string& joint_prefix)
{
  return joint_prefix + std::to_string(joint_number);
}
 
pilz_industrial_motion_planner::JointLimitsContainer createFakeLimits(const std::vector<std::string>& joint_names);
 
inline std::string demangle(const char* name)
{
  return boost::core::demangle(name);
}
 
//********************************************
// Motion plan requests
//********************************************
 
inline sensor_msgs::msg::JointState generateJointState(const std::vector<double>& pos, const std::vector<double>& vel,
                                                       const std::string& joint_prefix = testutils::JOINT_NAME_PREFIX)
{
  sensor_msgs::msg::JointState state;
  auto posit = pos.cbegin();
  size_t i = 0;
 
  while (posit != pos.cend())
  {
    state.name.push_back(testutils::getJointName(i + 1, joint_prefix));
    state.position.push_back(*posit);
 
    i++;
    posit++;
  }
  for (const auto& one_vel : vel)
  {
    state.velocity.push_back(one_vel);
  }
  return state;
}
 
inline sensor_msgs::msg::JointState generateJointState(const std::vector<double>& pos,
                                                       const std::string& joint_prefix = testutils::JOINT_NAME_PREFIX)
{
  return generateJointState(pos, std::vector<double>(), joint_prefix);
}
 
inline moveit_msgs::msg::Constraints
generateJointConstraint(const std::vector<double>& pos_list,
                        const std::string& joint_prefix = testutils::JOINT_NAME_PREFIX)
{
  moveit_msgs::msg::Constraints gc;
 
  auto pos_it = pos_list.begin();
 
  for (size_t i = 0; i < pos_list.size(); ++i)
  {
    moveit_msgs::msg::JointConstraint jc;
    jc.joint_name = testutils::getJointName(i + 1, joint_prefix);
    jc.position = *pos_it;
    gc.joint_constraints.push_back(jc);
 
    ++pos_it;
  }
 
  return gc;
}
 
bool getExpectedGoalPose(const moveit::core::RobotModelConstPtr& robot_model,
                         const planning_interface::MotionPlanRequest& req, std::string& link_name,
                         Eigen::Isometry3d& goal_pose_expect);
 
void createDummyRequest(const moveit::core::RobotModelConstPtr& robot_model, const std::string& planning_group,
                        planning_interface::MotionPlanRequest& req);
 
void createPTPRequest(const std::string& planning_group, const moveit::core::RobotState& start_state,
                      const moveit::core::RobotState& goal_state, planning_interface::MotionPlanRequest& req);
 
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);
 
bool 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);
 
bool isGoalReached(const moveit::core::RobotModelConstPtr& robot_model,
                   const trajectory_msgs::msg::JointTrajectory& trajectory,
                   const planning_interface::MotionPlanRequest& req, const double tolerance);
 
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);
 
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);
 
inline int getWayPointIndex(const robot_trajectory::RobotTrajectoryPtr& trajectory, const double time_from_start)
{
  int index_before, index_after;
  double blend;
  trajectory->findWayPointIndicesForDurationAfterStart(time_from_start, index_before, index_after, blend);
  return blend > 0.5 ? index_after : index_before;
}
 
bool checkJointTrajectory(const trajectory_msgs::msg::JointTrajectory& trajectory,
                          const pilz_industrial_motion_planner::JointLimitsContainer& joint_limits);
 
::testing::AssertionResult hasStrictlyIncreasingTime(const robot_trajectory::RobotTrajectoryPtr& trajectory);
 
bool isTrajectoryConsistent(const trajectory_msgs::msg::JointTrajectory& trajectory);
 
bool isPositionBounded(const trajectory_msgs::msg::JointTrajectory& trajectory,
                       const pilz_industrial_motion_planner::JointLimitsContainer& joint_limits);
 
bool isVelocityBounded(const trajectory_msgs::msg::JointTrajectory& trajectory,
                       const pilz_industrial_motion_planner::JointLimitsContainer& joint_limits);
 
bool isAccelerationBounded(const trajectory_msgs::msg::JointTrajectory& trajectory,
                           const pilz_industrial_motion_planner::JointLimitsContainer& joint_limits);
 
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);
 
bool computeLinkFK(const moveit::core::RobotModelConstPtr& robot_model, const std::string& link_name,
                   const std::map<std::string, double>& joint_state, Eigen::Isometry3d& pose);
 
bool checkOriginalTrajectoryAfterBlending(const pilz_industrial_motion_planner::TrajectoryBlendRequest& req,
                                          const pilz_industrial_motion_planner::TrajectoryBlendResponse& res,
                                          const double time_tolerance);
 
bool checkBlendingJointSpaceContinuity(const pilz_industrial_motion_planner::TrajectoryBlendResponse& res,
                                       double joint_velocity_tolerance, double joint_accleration_tolerance);
 
bool checkBlendingCartSpaceContinuity(const pilz_industrial_motion_planner::TrajectoryBlendRequest& req,
                                      const pilz_industrial_motion_planner::TrajectoryBlendResponse& res,
                                      const pilz_industrial_motion_planner::LimitsContainer& planner_limits);
 
bool checkThatPointsInRadius(const std::string& link_name, double r, Eigen::Isometry3d& circ_pose,
                             const pilz_industrial_motion_planner::TrajectoryBlendResponse& res);
 
void computeCartVelocity(const Eigen::Isometry3d& pose_1, const Eigen::Isometry3d& pose_2, double duration,
                         Eigen::Vector3d& v, Eigen::Vector3d& w);
 
void getLinLinPosesWithoutOriChange(const std::string& frame_id, sensor_msgs::msg::JointState& initialJointState,
                                    geometry_msgs::msg::PoseStamped& p1, geometry_msgs::msg::PoseStamped& p2);
 
void getOriChange(Eigen::Matrix3d& ori1, Eigen::Matrix3d& ori2);
 
void createFakeCartTraj(const robot_trajectory::RobotTrajectoryPtr& traj, const std::string& link_name,
                        moveit_msgs::msg::RobotTrajectory& fake_traj);
 
inline geometry_msgs::msg::Quaternion fromEuler(double a, double b, double c)
{
  tf2::Vector3 qvz(0., 0., 1.);
  tf2::Vector3 qvy(0., 1., 0.);
  tf2::Quaternion q1(qvz, a);
 
  q1 = q1 * tf2::Quaternion(qvy, b);
  q1 = q1 * tf2::Quaternion(qvz, c);
 
  return tf2::toMsg(q1);
}
 
struct BlendTestData
{
  std::vector<double> start_position;
  std::vector<double> mid_position;
  std::vector<double> end_position;
};
 
bool getBlendTestData(const rclcpp::Node::SharedPtr& node, const size_t& dataset_num, const std::string& name_prefix,
                      std::vector<BlendTestData>& datasets);
 
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);
 
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, double sampling_time_1, double sampling_time_2,
                                   planning_interface::MotionPlanResponse& res_lin_1,
                                   planning_interface::MotionPlanResponse& res_lin_2, double& dis_lin_1,
                                   double& dis_lin_2);
 
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);
 
void checkRobotModel(const moveit::core::RobotModelConstPtr& robot_model, const std::string& group_name,
                     const std::string& link_name);
 
::testing::AssertionResult hasTrapezoidVelocity(std::vector<double> accelerations, const double acc_tol);
 
::testing::AssertionResult
checkCartesianTranslationalPath(const robot_trajectory::RobotTrajectoryConstPtr& trajectory,
                                const std::string& link_name,
                                const double acc_tol = DEFAULT_ACCELERATION_EQUALITY_TOLERANCE);
 
::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);
 
inline bool isMonotonouslyDecreasing(const std::vector<double>& vec, double tol)
{
  return std::is_sorted(vec.begin(), vec.end(), [tol](double a, double b) {
    return std::abs(a - b) >= tol && a >= b;  // true -> a is ordered before b -> list is not sorted
  });
}
 
}  // namespace testutils