From eb6d0fc489d08803d8be1d0931eed12a5d01115d Mon Sep 17 00:00:00 2001 From: Jon Meyer Date: Sat, 8 Jul 2017 00:25:53 -0500 Subject: [PATCH 1/3] Created a new descartes_tests package and moved everything into that directory. --- descartes_moveit/CMakeLists.txt | 16 -- descartes_planner/CMakeLists.txt | 18 -- descartes_tests/CMakeLists.txt | 82 +++++++++ .../descartes_tests}/cartesian_robot.h | 2 +- descartes_tests/package.xml | 56 ++++++ .../src}/cartesian_robot.cpp | 6 +- .../launch/moveit_state_adapter.launch | 8 +- .../test/moveit}/launch/utest.launch | 6 +- .../moveit}/moveit_state_adapter_test.cpp | 6 +- .../resources/kuka_kr210/joint_limits.yaml | 0 .../resources/kuka_kr210/kinematics.yaml | 0 .../resources/kuka_kr210/kr210l150.urdf | 0 .../resources/kuka_kr210/kuka_kr210.srdf | 0 .../test/moveit}/utest.cpp | 0 .../test/planner}/dense_planner.cpp | 0 .../test/planner}/planner_tests.h | 4 +- .../test/planner}/planning_graph_tests.cpp | 4 +- .../test/planner}/sparse_planner.cpp | 0 .../test/planner}/utest.cpp | 0 .../test/planner}/utils/trajectory_maker.cpp | 0 .../test/planner}/utils/trajectory_maker.h | 0 .../test/trajectory}/axial_symmetric_pt.cpp | 4 +- .../test/trajectory/cart_trajectory_pt.cpp | 82 +++++++++ .../test/trajectory}/cartesian_robot_test.cpp | 10 +- .../test/trajectory}/joint_trajectory_pt.cpp | 0 .../test/trajectory}/robot_model_test.hpp | 4 +- descartes_tests/test/trajectory/utest.cpp | 25 +++ descartes_trajectory/CMakeLists.txt | 15 +- .../test/cart_trajectory_pt.cpp | 160 ------------------ 29 files changed, 276 insertions(+), 232 deletions(-) create mode 100644 descartes_tests/CMakeLists.txt rename {descartes_trajectory/test/descartes_trajectory_test => descartes_tests/include/descartes_tests}/cartesian_robot.h (98%) create mode 100644 descartes_tests/package.xml rename {descartes_trajectory/test => descartes_tests/src}/cartesian_robot.cpp (97%) rename {descartes_moveit/test => descartes_tests/test/moveit}/launch/moveit_state_adapter.launch (70%) rename {descartes_moveit/test => descartes_tests/test/moveit}/launch/utest.launch (57%) rename {descartes_moveit/test => descartes_tests/test/moveit}/moveit_state_adapter_test.cpp (92%) rename {descartes_moveit/test => descartes_tests/test/moveit}/resources/kuka_kr210/joint_limits.yaml (100%) rename {descartes_moveit/test => descartes_tests/test/moveit}/resources/kuka_kr210/kinematics.yaml (100%) rename {descartes_moveit/test => descartes_tests/test/moveit}/resources/kuka_kr210/kr210l150.urdf (100%) rename {descartes_moveit/test => descartes_tests/test/moveit}/resources/kuka_kr210/kuka_kr210.srdf (100%) rename {descartes_moveit/test => descartes_tests/test/moveit}/utest.cpp (100%) rename {descartes_planner/test => descartes_tests/test/planner}/dense_planner.cpp (100%) rename {descartes_planner/test => descartes_tests/test/planner}/planner_tests.h (96%) rename {descartes_planner/test => descartes_tests/test/planner}/planning_graph_tests.cpp (96%) rename {descartes_planner/test => descartes_tests/test/planner}/sparse_planner.cpp (100%) rename {descartes_planner/test => descartes_tests/test/planner}/utest.cpp (100%) rename {descartes_planner/test => descartes_tests/test/planner}/utils/trajectory_maker.cpp (100%) rename {descartes_planner/test => descartes_tests/test/planner}/utils/trajectory_maker.h (100%) rename {descartes_trajectory/test => descartes_tests/test/trajectory}/axial_symmetric_pt.cpp (97%) create mode 100644 descartes_tests/test/trajectory/cart_trajectory_pt.cpp rename {descartes_trajectory/test => descartes_tests/test/trajectory}/cartesian_robot_test.cpp (78%) rename {descartes_trajectory/test => descartes_tests/test/trajectory}/joint_trajectory_pt.cpp (100%) rename {descartes_trajectory/test/descartes_trajectory_test => descartes_tests/test/trajectory}/robot_model_test.hpp (98%) create mode 100644 descartes_tests/test/trajectory/utest.cpp delete mode 100644 descartes_trajectory/test/cart_trajectory_pt.cpp diff --git a/descartes_moveit/CMakeLists.txt b/descartes_moveit/CMakeLists.txt index 2ce87379..c5a4d1f3 100644 --- a/descartes_moveit/CMakeLists.txt +++ b/descartes_moveit/CMakeLists.txt @@ -59,22 +59,6 @@ target_link_libraries(descartes_moveit ${catkin_LIBRARIES} ) - -############# -## Testing ## -############# - -if(CATKIN_ENABLE_TESTING) - find_package(rostest) - set(UTEST_SRC_FILES test/utest.cpp - test/moveit_state_adapter_test.cpp) - - add_rostest_gtest(${PROJECT_NAME}_utest test/launch/utest.launch ${UTEST_SRC_FILES}) - target_compile_definitions(${PROJECT_NAME}_utest PUBLIC GTEST_USE_OWN_TR1_TUPLE=0) - target_link_libraries(${PROJECT_NAME}_utest descartes_moveit ${catkin_LIBRARIES} ${Boost_LIBRARIES}) -endif() - - ############# ## Install ## ############# diff --git a/descartes_planner/CMakeLists.txt b/descartes_planner/CMakeLists.txt index f52e7ad1..e75f4ff3 100644 --- a/descartes_planner/CMakeLists.txt +++ b/descartes_planner/CMakeLists.txt @@ -26,7 +26,6 @@ endif() catkin_package( INCLUDE_DIRS include - test LIBRARIES descartes_planner CATKIN_DEPENDS @@ -72,20 +71,3 @@ install( install( DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) - - -############# -## Testing ## -############# -if(CATKIN_ENABLE_TESTING) - - set(UTEST_PLANNER_SRC_FILES - test/utest.cpp - test/dense_planner.cpp - test/sparse_planner.cpp - test/planning_graph_tests.cpp - test/utils/trajectory_maker.cpp) - catkin_add_gtest(${PROJECT_NAME}_planner_utest ${UTEST_PLANNER_SRC_FILES}) - target_link_libraries(${PROJECT_NAME}_planner_utest descartes_planner) - -endif() diff --git a/descartes_tests/CMakeLists.txt b/descartes_tests/CMakeLists.txt new file mode 100644 index 00000000..ed1ec65e --- /dev/null +++ b/descartes_tests/CMakeLists.txt @@ -0,0 +1,82 @@ +cmake_minimum_required(VERSION 2.8.3) +project(descartes_tests) + +add_compile_options(-std=c++11) + +find_package(catkin REQUIRED COMPONENTS + descartes_moveit + descartes_planner + descartes_trajectory +) + +catkin_package( + INCLUDE_DIRS + include + LIBRARIES + ${PROJECT_NAME} + CATKIN_DEPENDS + descartes_moveit + descartes_planner + descartes_trajectory +) + +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +add_library(${PROJECT_NAME} + src/cartesian_robot.cpp +) + +add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +target_link_libraries(${PROJECT_NAME} + ${catkin_LIBRARIES} +) + +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" +) + +if(CATKIN_ENABLE_TESTING) + # Trajectory Unit Tests + catkin_add_gtest(${PROJECT_NAME}_trajectory_utest + test/trajectory/utest.cpp + test/trajectory/axial_symmetric_pt.cpp + test/trajectory/cart_trajectory_pt.cpp + test/trajectory/joint_trajectory_pt.cpp + test/trajectory/cartesian_robot_test.cpp + ) + target_compile_definitions(${PROJECT_NAME}_trajectory_utest PUBLIC GTEST_USE_OWN_TR1_TUPLE=0) + target_link_libraries(${PROJECT_NAME}_trajectory_utest ${PROJECT_NAME}) + + # Planner unit tests + catkin_add_gtest(${PROJECT_NAME}_planner_utest + test/planner/utest.cpp + test/planner/dense_planner.cpp + test/planner/sparse_planner.cpp + test/planner/planning_graph_tests.cpp + test/planner/utils/trajectory_maker.cpp + ) + target_compile_definitions(${PROJECT_NAME}_planner_utest PUBLIC GTEST_USE_OWN_TR1_TUPLE=0) + target_link_libraries(${PROJECT_NAME}_planner_utest ${PROJECT_NAME}) + + # Moveit adapter unit tests + find_package(rostest) + add_rostest_gtest(${PROJECT_NAME}_moveit_utest + test/moveit/launch/utest.launch + test/moveit/utest.cpp + test/moveit/moveit_state_adapter_test.cpp + ) + target_compile_definitions(${PROJECT_NAME}_moveit_utest PUBLIC GTEST_USE_OWN_TR1_TUPLE=0) + target_link_libraries(${PROJECT_NAME}_moveit_utest ${PROJECT_NAME}) + +endif() diff --git a/descartes_trajectory/test/descartes_trajectory_test/cartesian_robot.h b/descartes_tests/include/descartes_tests/cartesian_robot.h similarity index 98% rename from descartes_trajectory/test/descartes_trajectory_test/cartesian_robot.h rename to descartes_tests/include/descartes_tests/cartesian_robot.h index 75855d3e..adf99de2 100644 --- a/descartes_trajectory/test/descartes_trajectory_test/cartesian_robot.h +++ b/descartes_tests/include/descartes_tests/cartesian_robot.h @@ -21,7 +21,7 @@ #include "descartes_core/robot_model.h" -namespace descartes_trajectory_test +namespace descartes_tests { /**@brief Cartesian Robot used for test purposes. This is a simple robot with simple kinematics. Each * joint corresponds to a cartesian direction (i.e. x, y, R, P, Y) (don't ask me how this is built, it diff --git a/descartes_tests/package.xml b/descartes_tests/package.xml new file mode 100644 index 00000000..e61bdd21 --- /dev/null +++ b/descartes_tests/package.xml @@ -0,0 +1,56 @@ + + + descartes_tests + 0.0.0 + The descartes_tests package + + + + + jon + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + descartes_moveit + descartes_planner + descartes_trajectory + descartes_moveit + descartes_planner + descartes_trajectory + + + + + + + + diff --git a/descartes_trajectory/test/cartesian_robot.cpp b/descartes_tests/src/cartesian_robot.cpp similarity index 97% rename from descartes_trajectory/test/cartesian_robot.cpp rename to descartes_tests/src/cartesian_robot.cpp index e7ad67bf..f2f42353 100644 --- a/descartes_trajectory/test/cartesian_robot.cpp +++ b/descartes_tests/src/cartesian_robot.cpp @@ -16,13 +16,13 @@ * limitations under the License. */ -#include "descartes_trajectory_test/cartesian_robot.h" +#include "descartes_tests/cartesian_robot.h" #include "descartes_core/pretty_print.hpp" #include "eigen_conversions/eigen_kdl.h" #include "ros/console.h" #include "ros/assert.h" -namespace descartes_trajectory_test +namespace descartes_tests { const static int DOF = 6; @@ -160,4 +160,4 @@ bool CartesianRobot::isValid(const Eigen::Affine3d &pose) const } -} // descartes_trajectory_test +} diff --git a/descartes_moveit/test/launch/moveit_state_adapter.launch b/descartes_tests/test/moveit/launch/moveit_state_adapter.launch similarity index 70% rename from descartes_moveit/test/launch/moveit_state_adapter.launch rename to descartes_tests/test/moveit/launch/moveit_state_adapter.launch index 811b8ac5..6925333b 100644 --- a/descartes_moveit/test/launch/moveit_state_adapter.launch +++ b/descartes_tests/test/moveit/launch/moveit_state_adapter.launch @@ -6,7 +6,7 @@ - + @@ -15,16 +15,16 @@ - + - + - + diff --git a/descartes_moveit/test/launch/utest.launch b/descartes_tests/test/moveit/launch/utest.launch similarity index 57% rename from descartes_moveit/test/launch/utest.launch rename to descartes_tests/test/moveit/launch/utest.launch index 299b5507..836e2a0c 100644 --- a/descartes_moveit/test/launch/utest.launch +++ b/descartes_tests/test/moveit/launch/utest.launch @@ -9,13 +9,13 @@ - + - + - + diff --git a/descartes_moveit/test/moveit_state_adapter_test.cpp b/descartes_tests/test/moveit/moveit_state_adapter_test.cpp similarity index 92% rename from descartes_moveit/test/moveit_state_adapter_test.cpp rename to descartes_tests/test/moveit/moveit_state_adapter_test.cpp index 28e2de0b..9223c087 100644 --- a/descartes_moveit/test/moveit_state_adapter_test.cpp +++ b/descartes_tests/test/moveit/moveit_state_adapter_test.cpp @@ -17,9 +17,9 @@ */ #include "descartes_moveit/moveit_state_adapter.h" -#include #include "moveit/robot_model_loader/robot_model_loader.h" #include +#include "../trajectory/robot_model_test.hpp" using namespace descartes_moveit; using namespace descartes_trajectory; @@ -27,7 +27,7 @@ using namespace descartes_core; using testing::Types; -namespace descartes_trajectory_test +namespace descartes_tests { // This variable must be global in order for the test to pass. // Destructing the robot model results in a boost mutex exception: @@ -56,7 +56,7 @@ RobotModelPtr CreateRobotModel() } template -class MoveitRobotModelTest : public descartes_trajectory_test::RobotModelTest +class MoveitRobotModelTest : public descartes_tests::RobotModelTest { }; diff --git a/descartes_moveit/test/resources/kuka_kr210/joint_limits.yaml b/descartes_tests/test/moveit/resources/kuka_kr210/joint_limits.yaml similarity index 100% rename from descartes_moveit/test/resources/kuka_kr210/joint_limits.yaml rename to descartes_tests/test/moveit/resources/kuka_kr210/joint_limits.yaml diff --git a/descartes_moveit/test/resources/kuka_kr210/kinematics.yaml b/descartes_tests/test/moveit/resources/kuka_kr210/kinematics.yaml similarity index 100% rename from descartes_moveit/test/resources/kuka_kr210/kinematics.yaml rename to descartes_tests/test/moveit/resources/kuka_kr210/kinematics.yaml diff --git a/descartes_moveit/test/resources/kuka_kr210/kr210l150.urdf b/descartes_tests/test/moveit/resources/kuka_kr210/kr210l150.urdf similarity index 100% rename from descartes_moveit/test/resources/kuka_kr210/kr210l150.urdf rename to descartes_tests/test/moveit/resources/kuka_kr210/kr210l150.urdf diff --git a/descartes_moveit/test/resources/kuka_kr210/kuka_kr210.srdf b/descartes_tests/test/moveit/resources/kuka_kr210/kuka_kr210.srdf similarity index 100% rename from descartes_moveit/test/resources/kuka_kr210/kuka_kr210.srdf rename to descartes_tests/test/moveit/resources/kuka_kr210/kuka_kr210.srdf diff --git a/descartes_moveit/test/utest.cpp b/descartes_tests/test/moveit/utest.cpp similarity index 100% rename from descartes_moveit/test/utest.cpp rename to descartes_tests/test/moveit/utest.cpp diff --git a/descartes_planner/test/dense_planner.cpp b/descartes_tests/test/planner/dense_planner.cpp similarity index 100% rename from descartes_planner/test/dense_planner.cpp rename to descartes_tests/test/planner/dense_planner.cpp diff --git a/descartes_planner/test/planner_tests.h b/descartes_tests/test/planner/planner_tests.h similarity index 96% rename from descartes_planner/test/planner_tests.h rename to descartes_tests/test/planner/planner_tests.h index af5ceb05..03985782 100644 --- a/descartes_planner/test/planner_tests.h +++ b/descartes_tests/test/planner/planner_tests.h @@ -2,7 +2,7 @@ #include #include -#include +#include #include "utils/trajectory_maker.h" @@ -20,7 +20,7 @@ class PathPlannerTest : public testing::Test } PathPlannerTest() - : velocity_limits_(6, 1.0), robot_(new descartes_trajectory_test::CartesianRobot(5.0, 0.001, velocity_limits_)) + : velocity_limits_(6, 1.0), robot_(new descartes_tests::CartesianRobot(5.0, 0.001, velocity_limits_)) { ros::Time::init(); } diff --git a/descartes_planner/test/planning_graph_tests.cpp b/descartes_tests/test/planner/planning_graph_tests.cpp similarity index 96% rename from descartes_planner/test/planning_graph_tests.cpp rename to descartes_tests/test/planner/planning_graph_tests.cpp index b54baa53..6aa07ffe 100644 --- a/descartes_planner/test/planning_graph_tests.cpp +++ b/descartes_tests/test/planner/planning_graph_tests.cpp @@ -18,7 +18,7 @@ #include #include -#include +#include #include #include @@ -28,7 +28,7 @@ static boost::shared_ptr makeTestRobot() const auto max_joint_vel = 1.0; const auto dof = 6; return boost::shared_ptr( - new descartes_trajectory_test::CartesianRobot(5.0, 0.001, std::vector(dof, max_joint_vel)) + new descartes_tests::CartesianRobot(5.0, 0.001, std::vector(dof, max_joint_vel)) ); } diff --git a/descartes_planner/test/sparse_planner.cpp b/descartes_tests/test/planner/sparse_planner.cpp similarity index 100% rename from descartes_planner/test/sparse_planner.cpp rename to descartes_tests/test/planner/sparse_planner.cpp diff --git a/descartes_planner/test/utest.cpp b/descartes_tests/test/planner/utest.cpp similarity index 100% rename from descartes_planner/test/utest.cpp rename to descartes_tests/test/planner/utest.cpp diff --git a/descartes_planner/test/utils/trajectory_maker.cpp b/descartes_tests/test/planner/utils/trajectory_maker.cpp similarity index 100% rename from descartes_planner/test/utils/trajectory_maker.cpp rename to descartes_tests/test/planner/utils/trajectory_maker.cpp diff --git a/descartes_planner/test/utils/trajectory_maker.h b/descartes_tests/test/planner/utils/trajectory_maker.h similarity index 100% rename from descartes_planner/test/utils/trajectory_maker.h rename to descartes_tests/test/planner/utils/trajectory_maker.h diff --git a/descartes_trajectory/test/axial_symmetric_pt.cpp b/descartes_tests/test/trajectory/axial_symmetric_pt.cpp similarity index 97% rename from descartes_trajectory/test/axial_symmetric_pt.cpp rename to descartes_tests/test/trajectory/axial_symmetric_pt.cpp index fe3196a3..70497dca 100644 --- a/descartes_trajectory/test/axial_symmetric_pt.cpp +++ b/descartes_tests/test/trajectory/axial_symmetric_pt.cpp @@ -18,11 +18,11 @@ #include "descartes_trajectory/axial_symmetric_pt.h" #include "descartes_core/utils.h" -#include "descartes_trajectory_test/cartesian_robot.h" +#include "descartes_tests/cartesian_robot.h" #include using namespace descartes_trajectory; -using namespace descartes_trajectory_test; +using namespace descartes_tests; // Does it have a default constructor TEST(AxialSymPt, construction) diff --git a/descartes_tests/test/trajectory/cart_trajectory_pt.cpp b/descartes_tests/test/trajectory/cart_trajectory_pt.cpp new file mode 100644 index 00000000..2f1b861e --- /dev/null +++ b/descartes_tests/test/trajectory/cart_trajectory_pt.cpp @@ -0,0 +1,82 @@ +/* + * Software License Agreement (Apache License) + * + * Copyright (c) 2014, Southwest Research Institute + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "descartes_trajectory/cart_trajectory_pt.h" +#include "descartes_core/utils.h" +#include "descartes_tests/cartesian_robot.h" +#include + +using namespace descartes_trajectory; +using namespace descartes_core; +using namespace descartes_tests; + +TEST(CartTrajPt, construction) +{ + CartTrajectoryPt def(); +} + +TEST(CartTrajPt, zeroTolerance) +{ + ROS_INFO_STREAM("Initializing zero tolerance cartesian point"); + CartTrajectoryPt zero_tol_pos(TolerancedFrame(utils::toFrame(0, 0, 0, 0, 0, 0), + ToleranceBase::zeroTolerance(0.0, 0.0, 0.0), + ToleranceBase::zeroTolerance(0.0, 0.0, 0.0)), + 0, 0); + + EigenSTL::vector_Affine3d solutions; + std::vector > joint_solutions; + + CartesianRobot robot; + zero_tol_pos.getCartesianPoses(robot, solutions); + EXPECT_EQ(solutions.size(), 1); + zero_tol_pos.getJointPoses(robot, joint_solutions); + EXPECT_EQ(joint_solutions.size(), 1); +} + +TEST(CartTrajPt, closestJointPose) +{ + const double POS_TOL = 0.5f; + const double POS_INC = 0.2; + const double ORIENT_TOL = 1.0; + const double ORIENT_INC = 0.2; + CartesianRobot robot(10, 4); + + // declaring pose values + const double x = 4.0f; + const double y = 5.0f; + const double z = 2.0f; + const double rx = 0.0f; + const double ry = 0.0f; + const double rz = M_PI / 4; + std::vector joint_pose = { x, y, z, rx, ry, rz }; + Eigen::Affine3d frame_pose = descartes_core::utils::toFrame(x, y, z, rx, ry, rz); + + ROS_INFO_STREAM("Initializing tolerance cartesian point"); + CartTrajectoryPt cart_point( + TolerancedFrame(utils::toFrame(x, y, z, rx, ry, rz), + ToleranceBase::createSymmetric(x, y, z, POS_TOL), + ToleranceBase::createSymmetric(rx, ry, rz, ORIENT_TOL)), + POS_INC, ORIENT_INC); + + ROS_INFO_STREAM("Testing getClosestJointPose(...)"); + std::vector closest_joint_pose; + EXPECT_TRUE(cart_point.getClosestJointPose(joint_pose, robot, closest_joint_pose)); + + ROS_INFO_STREAM("Testing equality between seed joint pose and closest joint pose"); + EXPECT_EQ(joint_pose, closest_joint_pose); +} diff --git a/descartes_trajectory/test/cartesian_robot_test.cpp b/descartes_tests/test/trajectory/cartesian_robot_test.cpp similarity index 78% rename from descartes_trajectory/test/cartesian_robot_test.cpp rename to descartes_tests/test/trajectory/cartesian_robot_test.cpp index 3e2929db..e6a2273b 100644 --- a/descartes_trajectory/test/cartesian_robot_test.cpp +++ b/descartes_tests/test/trajectory/cartesian_robot_test.cpp @@ -16,14 +16,14 @@ * limitations under the License. */ -#include "descartes_trajectory_test/cartesian_robot.h" -#include "descartes_trajectory_test/robot_model_test.hpp" +#include "descartes_tests/cartesian_robot.h" +#include "robot_model_test.hpp" using namespace descartes_core; using testing::Types; -namespace descartes_trajectory_test +namespace descartes_tests { template <> RobotModelPtr CreateRobotModel() @@ -32,10 +32,10 @@ RobotModelPtr CreateRobotModel() } template -class CartesianRobotModelTest : public descartes_trajectory_test::RobotModelTest +class CartesianRobotModelTest : public descartes_tests::RobotModelTest { }; INSTANTIATE_TYPED_TEST_CASE_P(CartesianRobotModelTest, RobotModelTest, CartesianRobot); -} // descartes_trajectory_test +} // descartes_tests diff --git a/descartes_trajectory/test/joint_trajectory_pt.cpp b/descartes_tests/test/trajectory/joint_trajectory_pt.cpp similarity index 100% rename from descartes_trajectory/test/joint_trajectory_pt.cpp rename to descartes_tests/test/trajectory/joint_trajectory_pt.cpp diff --git a/descartes_trajectory/test/descartes_trajectory_test/robot_model_test.hpp b/descartes_tests/test/trajectory/robot_model_test.hpp similarity index 98% rename from descartes_trajectory/test/descartes_trajectory_test/robot_model_test.hpp rename to descartes_tests/test/trajectory/robot_model_test.hpp index 132109da..6dc3c5c1 100644 --- a/descartes_trajectory/test/descartes_trajectory_test/robot_model_test.hpp +++ b/descartes_tests/test/trajectory/robot_model_test.hpp @@ -33,7 +33,7 @@ */ -namespace descartes_trajectory_test +namespace descartes_tests { template descartes_core::RobotModelPtr CreateRobotModel(); @@ -114,6 +114,6 @@ TYPED_TEST_P(RobotModelTest, getAllIK) REGISTER_TYPED_TEST_CASE_P(RobotModelTest, construction, getIK, getAllIK); -} // descartes_trajectory_test +} // descartes_tests #endif // ROBOT_MODEL_TEST_HPP_ diff --git a/descartes_tests/test/trajectory/utest.cpp b/descartes_tests/test/trajectory/utest.cpp new file mode 100644 index 00000000..45bba49a --- /dev/null +++ b/descartes_tests/test/trajectory/utest.cpp @@ -0,0 +1,25 @@ +/* + * Software License Agreement (Apache License) + * + * Copyright (c) 2014, Southwest Research Institute + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/descartes_trajectory/CMakeLists.txt b/descartes_trajectory/CMakeLists.txt index 591e009f..5ddefdfc 100644 --- a/descartes_trajectory/CMakeLists.txt +++ b/descartes_trajectory/CMakeLists.txt @@ -24,7 +24,6 @@ endif() catkin_package( INCLUDE_DIRS include - test LIBRARIES descartes_trajectory CATKIN_DEPENDS @@ -51,7 +50,6 @@ add_library(descartes_trajectory src/joint_trajectory_pt.cpp src/trajectory.cpp src/axial_symmetric_pt.cpp - test/cartesian_robot.cpp ) target_link_libraries(descartes_trajectory @@ -77,16 +75,11 @@ install( ############# if(CATKIN_ENABLE_TESTING) - set(UTEST_SRC_FILES test/utest.cpp - test/trajectory_pt.cpp - test/cart_trajectory_pt.cpp - test/joint_trajectory_pt.cpp - test/cartesian_robot.cpp - test/cartesian_robot_test.cpp - test/axial_symmetric_pt.cpp) - + set(UTEST_SRC_FILES + test/utest.cpp + test/trajectory_pt.cpp + ) catkin_add_gtest(${PROJECT_NAME}_utest ${UTEST_SRC_FILES}) target_compile_definitions(${PROJECT_NAME}_utest PUBLIC GTEST_USE_OWN_TR1_TUPLE=0) target_link_libraries(${PROJECT_NAME}_utest descartes_trajectory) - endif() diff --git a/descartes_trajectory/test/cart_trajectory_pt.cpp b/descartes_trajectory/test/cart_trajectory_pt.cpp deleted file mode 100644 index c8a78a01..00000000 --- a/descartes_trajectory/test/cart_trajectory_pt.cpp +++ /dev/null @@ -1,160 +0,0 @@ -/* - * Software License Agreement (Apache License) - * - * Copyright (c) 2014, Southwest Research Institute - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include "descartes_trajectory/cart_trajectory_pt.h" -#include "descartes_core/utils.h" -#include "descartes_trajectory_test/cartesian_robot.h" -#include - -using namespace descartes_trajectory; -using namespace descartes_core; -using namespace descartes_trajectory_test; - -TEST(CartTrajPt, construction) -{ - CartTrajectoryPt def(); -} - -TEST(CartTrajPt, getPoses) -{ - const double POS_TOL = 2.0; - const double POS_INC = 0.2; - - const double ORIENT_TOL = 2 * M_PI; - const double ORIENT_INC = M_PI / 4; - - const double EPSILON = 0.001; - - const int NUM_SAMPLED_POS = pow((POS_TOL / POS_INC) + 1, 3.0); - const int NUM_SAMPLED_ORIENT = pow((ORIENT_TOL / ORIENT_INC) + 1, 3.0); - const int NUM_SAMPLED_BOTH = NUM_SAMPLED_POS * NUM_SAMPLED_ORIENT; - - ROS_INFO_STREAM("Expected samples, position: " << NUM_SAMPLED_POS << ", orientation: " << NUM_SAMPLED_ORIENT - << ", both: " << NUM_SAMPLED_BOTH); - - ROS_INFO_STREAM("Initializing fuzzy position point"); - CartTrajectoryPt fuzzy_pos( - TolerancedFrame(utils::toFrame(0, 0, 0, 0, 0, 0), - ToleranceBase::createSymmetric(0.0, 0.0, 0.0, POS_TOL + EPSILON), - ToleranceBase::createSymmetric(0.0, 0.0, 0.0, 0.0)), - POS_INC, ORIENT_INC); - - ROS_INFO_STREAM("Initializing fuzzy orientation point"); - CartTrajectoryPt fuzzy_orient( - TolerancedFrame(utils::toFrame(0, 0, 0, 0, 0, 0), - ToleranceBase::createSymmetric(0.0, 0.0, 0.0, 0.0), - ToleranceBase::createSymmetric(0.0, 0.0, 0.0, ORIENT_TOL + EPSILON)), - POS_INC, ORIENT_INC); - - ROS_INFO_STREAM("Initializing fuzzy position/orientation point"); - CartTrajectoryPt fuzzy_both( - TolerancedFrame(utils::toFrame(0, 0, 0, 0, 0, 0), - ToleranceBase::createSymmetric(0.0, 0.0, 0.0, POS_TOL + EPSILON), - ToleranceBase::createSymmetric(0.0, 0.0, 0.0, ORIENT_TOL + EPSILON)), - POS_INC, ORIENT_INC); - - EigenSTL::vector_Affine3d solutions; - std::vector > joint_solutions; - - ROS_INFO_STREAM("Testing fuzzy pos point"); - CartesianRobot robot(POS_TOL + 2 * EPSILON, ORIENT_TOL + 2 * EPSILON); - fuzzy_pos.getCartesianPoses(robot, solutions); - EXPECT_EQ(solutions.size(), NUM_SAMPLED_POS); - fuzzy_pos.getJointPoses(robot, joint_solutions); - EXPECT_EQ(joint_solutions.size(), NUM_SAMPLED_POS); - - ROS_INFO_STREAM("Testing fuzzy orient point"); - fuzzy_orient.getCartesianPoses(robot, solutions); - EXPECT_EQ(solutions.size(), NUM_SAMPLED_ORIENT); - fuzzy_orient.getJointPoses(robot, joint_solutions); - EXPECT_EQ(joint_solutions.size(), NUM_SAMPLED_ORIENT); - - ROS_INFO_STREAM("Testing fuzzy both point"); - fuzzy_both.getCartesianPoses(robot, solutions); - EXPECT_EQ(solutions.size(), NUM_SAMPLED_BOTH); - fuzzy_both.getJointPoses(robot, joint_solutions); - EXPECT_EQ(joint_solutions.size(), NUM_SAMPLED_BOTH); - - // TODO: Add tests for cartesian points outside of the robot workspace. These - // tests below seem to work, but predicting the number of samples isn't working. - // const double WS_FRACTION = 0.5; //Reduces robot workspace - // const int LIMIT_SAMPLED_POS = pow(((WS_FRACTION * POS_TOL)/POS_INC) + 1, 3.0); - // const int LIMIT_SAMPLED_ORIENT = pow(((WS_FRACTION * ORIENT_TOL)/ORIENT_INC) + 1, 3.0); - // const int LIMIT_SAMPLED_BOTH = LIMIT_SAMPLED_POS * LIMIT_SAMPLED_ORIENT; - - // CartesianRobot limited_robot(POS_TOL * WS_FRACTION + 2*EPSILON, ORIENT_TOL * WS_FRACTION + 2*EPSILON); - // fuzzy_pos.getCartesianPoses(limited_robot, solutions); - // EXPECT_EQ(solutions.size(), LIMIT_SAMPLED_POS); - - // fuzzy_orient.getCartesianPoses(limited_robot, solutions); - // EXPECT_EQ(solutions.size(), LIMIT_SAMPLED_ORIENT); - - // fuzzy_both.getCartesianPoses(limited_robot, solutions); - // EXPECT_EQ(solutions.size(), LIMIT_SAMPLED_BOTH); -} - -TEST(CartTrajPt, zeroTolerance) -{ - ROS_INFO_STREAM("Initializing zero tolerance cartesian point"); - CartTrajectoryPt zero_tol_pos(TolerancedFrame(utils::toFrame(0, 0, 0, 0, 0, 0), - ToleranceBase::zeroTolerance(0.0, 0.0, 0.0), - ToleranceBase::zeroTolerance(0.0, 0.0, 0.0)), - 0, 0); - - EigenSTL::vector_Affine3d solutions; - std::vector > joint_solutions; - - CartesianRobot robot; - zero_tol_pos.getCartesianPoses(robot, solutions); - EXPECT_EQ(solutions.size(), 1); - zero_tol_pos.getJointPoses(robot, joint_solutions); - EXPECT_EQ(joint_solutions.size(), 1); -} - -TEST(CartTrajPt, closestJointPose) -{ - const double POS_TOL = 0.5f; - const double POS_INC = 0.2; - const double ORIENT_TOL = 1.0; - const double ORIENT_INC = 0.2; - CartesianRobot robot(10, 4); - - // declaring pose values - const double x = 4.0f; - const double y = 5.0f; - const double z = 2.0f; - const double rx = 0.0f; - const double ry = 0.0f; - const double rz = M_PI / 4; - std::vector joint_pose = { x, y, z, rx, ry, rz }; - Eigen::Affine3d frame_pose = descartes_core::utils::toFrame(x, y, z, rx, ry, rz); - - ROS_INFO_STREAM("Initializing tolerance cartesian point"); - CartTrajectoryPt cart_point( - TolerancedFrame(utils::toFrame(x, y, z, rx, ry, rz), - ToleranceBase::createSymmetric(x, y, z, POS_TOL), - ToleranceBase::createSymmetric(rx, ry, rz, ORIENT_TOL)), - POS_INC, ORIENT_INC); - - ROS_INFO_STREAM("Testing getClosestJointPose(...)"); - std::vector closest_joint_pose; - EXPECT_TRUE(cart_point.getClosestJointPose(joint_pose, robot, closest_joint_pose)); - - ROS_INFO_STREAM("Testing equality between seed joint pose and closest joint pose"); - EXPECT_EQ(joint_pose, closest_joint_pose); -} From d4f914142ca84c56cd3b1e98687017bcad196a8f Mon Sep 17 00:00:00 2001 From: Jon Meyer Date: Sat, 8 Jul 2017 00:45:17 -0500 Subject: [PATCH 2/3] Updated package.xml files for both the tests pkg and the meta-pkg --- descartes/package.xml | 2 +- descartes_tests/package.xml | 50 ++++++------------------------------- 2 files changed, 9 insertions(+), 43 deletions(-) diff --git a/descartes/package.xml b/descartes/package.xml index f9ca9663..a66a0408 100644 --- a/descartes/package.xml +++ b/descartes/package.xml @@ -16,7 +16,7 @@ descartes_planner descartes_trajectory descartes_utilities - + descartes_tests diff --git a/descartes_tests/package.xml b/descartes_tests/package.xml index e61bdd21..a3da5a0e 100644 --- a/descartes_tests/package.xml +++ b/descartes_tests/package.xml @@ -1,44 +1,16 @@ descartes_tests - 0.0.0 - The descartes_tests package + 0.1.0 + + A package dedicated to unit tests for the Descartes cartesian path + planning library. + - - - - jon + Jonathan Meyer + Apache 2.0 - - - - - TODO - - - - - - - - - - - - - - - - - - - - - - - - - + gtest catkin descartes_moveit descartes_planner @@ -47,10 +19,4 @@ descartes_planner descartes_trajectory - - - - - - From 2cbd771574b291f6ef049ed30011930deab41135 Mon Sep 17 00:00:00 2001 From: Jon Meyer Date: Sat, 8 Jul 2017 00:26:29 -0500 Subject: [PATCH 3/3] Re-enabled install space testing and made an assorted of changes post review --- .travis.yml | 3 - descartes/package.xml | 1 - .../test/trajectory/cart_trajectory_pt.cpp | 78 +++++++++++++++++++ 3 files changed, 78 insertions(+), 4 deletions(-) diff --git a/.travis.yml b/.travis.yml index 4a2dd258..19e4a530 100644 --- a/.travis.yml +++ b/.travis.yml @@ -6,10 +6,7 @@ compiler: notifications: email: on_failure: always - env: - global: - - BEFORE_SCRIPT='catkin config -w $CATKIN_WORKSPACE --no-install' matrix: - USE_DEB=true ROS_DISTRO="kinetic" diff --git a/descartes/package.xml b/descartes/package.xml index a66a0408..52cad428 100644 --- a/descartes/package.xml +++ b/descartes/package.xml @@ -16,7 +16,6 @@ descartes_planner descartes_trajectory descartes_utilities - descartes_tests diff --git a/descartes_tests/test/trajectory/cart_trajectory_pt.cpp b/descartes_tests/test/trajectory/cart_trajectory_pt.cpp index 2f1b861e..38c2d329 100644 --- a/descartes_tests/test/trajectory/cart_trajectory_pt.cpp +++ b/descartes_tests/test/trajectory/cart_trajectory_pt.cpp @@ -80,3 +80,81 @@ TEST(CartTrajPt, closestJointPose) ROS_INFO_STREAM("Testing equality between seed joint pose and closest joint pose"); EXPECT_EQ(joint_pose, closest_joint_pose); } + +TEST(CartTrajPt, getPoses) +{ + const double POS_TOL = 2.0; + const double POS_INC = 0.2; + + const double ORIENT_TOL = 2 * M_PI; + const double ORIENT_INC = M_PI / 4; + + const double EPSILON = 0.001; + + const int NUM_SAMPLED_POS = pow((POS_TOL / POS_INC) + 1, 3.0); + const int NUM_SAMPLED_ORIENT = pow((ORIENT_TOL / ORIENT_INC) + 1, 3.0); + const int NUM_SAMPLED_BOTH = NUM_SAMPLED_POS * NUM_SAMPLED_ORIENT; + + ROS_INFO_STREAM("Expected samples, position: " << NUM_SAMPLED_POS << ", orientation: " << NUM_SAMPLED_ORIENT + << ", both: " << NUM_SAMPLED_BOTH); + + ROS_INFO_STREAM("Initializing fuzzy position point"); + CartTrajectoryPt fuzzy_pos( + TolerancedFrame(utils::toFrame(0, 0, 0, 0, 0, 0), + ToleranceBase::createSymmetric(0.0, 0.0, 0.0, POS_TOL + EPSILON), + ToleranceBase::createSymmetric(0.0, 0.0, 0.0, 0.0)), + POS_INC, ORIENT_INC); + + ROS_INFO_STREAM("Initializing fuzzy orientation point"); + CartTrajectoryPt fuzzy_orient( + TolerancedFrame(utils::toFrame(0, 0, 0, 0, 0, 0), + ToleranceBase::createSymmetric(0.0, 0.0, 0.0, 0.0), + ToleranceBase::createSymmetric(0.0, 0.0, 0.0, ORIENT_TOL + EPSILON)), + POS_INC, ORIENT_INC); + + ROS_INFO_STREAM("Initializing fuzzy position/orientation point"); + CartTrajectoryPt fuzzy_both( + TolerancedFrame(utils::toFrame(0, 0, 0, 0, 0, 0), + ToleranceBase::createSymmetric(0.0, 0.0, 0.0, POS_TOL + EPSILON), + ToleranceBase::createSymmetric(0.0, 0.0, 0.0, ORIENT_TOL + EPSILON)), + POS_INC, ORIENT_INC); + + EigenSTL::vector_Affine3d solutions; + std::vector > joint_solutions; + + ROS_INFO_STREAM("Testing fuzzy pos point"); + CartesianRobot robot(POS_TOL + 2 * EPSILON, ORIENT_TOL + 2 * EPSILON); + fuzzy_pos.getCartesianPoses(robot, solutions); + EXPECT_EQ(solutions.size(), NUM_SAMPLED_POS); + fuzzy_pos.getJointPoses(robot, joint_solutions); + EXPECT_EQ(joint_solutions.size(), NUM_SAMPLED_POS); + + ROS_INFO_STREAM("Testing fuzzy orient point"); + fuzzy_orient.getCartesianPoses(robot, solutions); + EXPECT_EQ(solutions.size(), NUM_SAMPLED_ORIENT); + fuzzy_orient.getJointPoses(robot, joint_solutions); + EXPECT_EQ(joint_solutions.size(), NUM_SAMPLED_ORIENT); + + ROS_INFO_STREAM("Testing fuzzy both point"); + fuzzy_both.getCartesianPoses(robot, solutions); + EXPECT_EQ(solutions.size(), NUM_SAMPLED_BOTH); + fuzzy_both.getJointPoses(robot, joint_solutions); + EXPECT_EQ(joint_solutions.size(), NUM_SAMPLED_BOTH); + + // TODO: Add tests for cartesian points outside of the robot workspace. These + // tests below seem to work, but predicting the number of samples isn't working. + // const double WS_FRACTION = 0.5; //Reduces robot workspace + // const int LIMIT_SAMPLED_POS = pow(((WS_FRACTION * POS_TOL)/POS_INC) + 1, 3.0); + // const int LIMIT_SAMPLED_ORIENT = pow(((WS_FRACTION * ORIENT_TOL)/ORIENT_INC) + 1, 3.0); + // const int LIMIT_SAMPLED_BOTH = LIMIT_SAMPLED_POS * LIMIT_SAMPLED_ORIENT; + + // CartesianRobot limited_robot(POS_TOL * WS_FRACTION + 2*EPSILON, ORIENT_TOL * WS_FRACTION + 2*EPSILON); + // fuzzy_pos.getCartesianPoses(limited_robot, solutions); + // EXPECT_EQ(solutions.size(), LIMIT_SAMPLED_POS); + + // fuzzy_orient.getCartesianPoses(limited_robot, solutions); + // EXPECT_EQ(solutions.size(), LIMIT_SAMPLED_ORIENT); + + // fuzzy_both.getCartesianPoses(limited_robot, solutions); + // EXPECT_EQ(solutions.size(), LIMIT_SAMPLED_BOTH); +}