From 6c9b68e7e580f578c24625e03e396807c341464b Mon Sep 17 00:00:00 2001 From: Immanuel Martini Date: Mon, 3 Aug 2020 21:13:01 +0200 Subject: [PATCH 1/3] Move ikfast plugin test to prbt_moveit_config --- prbt_ikfast_manipulator_plugin/CMakeLists.txt | 52 ------------------- prbt_ikfast_manipulator_plugin/package.xml | 5 -- prbt_moveit_config/CMakeLists.txt | 39 ++++++++++++++ prbt_moveit_config/package.xml | 6 +++ .../tst_prbt_ikfast_manipulator_plugin.cpp | 0 .../tst_prbt_ikfast_manipulator_plugin.test | 4 +- 6 files changed, 47 insertions(+), 59 deletions(-) rename {prbt_ikfast_manipulator_plugin/test/unit_tests => prbt_moveit_config/test}/tst_prbt_ikfast_manipulator_plugin.cpp (100%) rename {prbt_ikfast_manipulator_plugin/test/unit_tests => prbt_moveit_config/test}/tst_prbt_ikfast_manipulator_plugin.test (75%) diff --git a/prbt_ikfast_manipulator_plugin/CMakeLists.txt b/prbt_ikfast_manipulator_plugin/CMakeLists.txt index d7e58c70a..6925ed932 100644 --- a/prbt_ikfast_manipulator_plugin/CMakeLists.txt +++ b/prbt_ikfast_manipulator_plugin/CMakeLists.txt @@ -36,23 +36,6 @@ target_link_libraries(${IKFAST_LIBRARY_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARI # suppress warnings about unused variables in OpenRave's solver code target_compile_options(${IKFAST_LIBRARY_NAME} PRIVATE -Wno-unused-variable) -################ -## Clang tidy ## -################ -if(CATKIN_ENABLE_CLANG_TIDY) - find_program( - CLANG_TIDY_EXE - NAMES "clang-tidy" - DOC "Path to clang-tidy executable" - ) - if(NOT CLANG_TIDY_EXE) - message(FATAL_ERROR "clang-tidy not found.") - else() - message(STATUS "clang-tidy found: ${CLANG_TIDY_EXE}") - set(CMAKE_CXX_CLANG_TIDY "${CLANG_TIDY_EXE}") - endif() -endif() - install(TARGETS ${IKFAST_LIBRARY_NAME} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} @@ -65,38 +48,3 @@ install( DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) - -# unittest of ikfast plugin -if(CATKIN_ENABLE_TESTING) - - find_package(rostest REQUIRED) - - if(ENABLE_COVERAGE_TESTING) - find_package(code_coverage REQUIRED) - APPEND_COVERAGE_COMPILER_FLAGS() - endif() - - find_package(moveit_ros_planning REQUIRED) - - include_directories(include ${catkin_INCLUDE_DIR}) - - add_rostest_gtest(unittest_${PROJECT_NAME} - test/unit_tests/tst_${PROJECT_NAME}.test - test/unit_tests/tst_${PROJECT_NAME}.cpp - ) - - target_link_libraries(unittest_${PROJECT_NAME} - ${catkin_LIBRARIES} - ${moveit_ros_planning_LIBRARIES} - ) - - # run: catkin_make -DENABLE_COVERAGE_TESTING=ON package_name_coverage - if(ENABLE_COVERAGE_TESTING) - set(COVERAGE_EXCLUDES "*/${PROJECT_NAME}/test*") - add_code_coverage( - NAME ${PROJECT_NAME}_coverage - DEPENDS tests - ) - endif() - -endif() diff --git a/prbt_ikfast_manipulator_plugin/package.xml b/prbt_ikfast_manipulator_plugin/package.xml index 5d6edd582..ade7a4589 100644 --- a/prbt_ikfast_manipulator_plugin/package.xml +++ b/prbt_ikfast_manipulator_plugin/package.xml @@ -26,9 +26,4 @@ pluginlib roscpp tf2_kdl - - moveit_ros_planning - rostest - rosunit - code_coverage diff --git a/prbt_moveit_config/CMakeLists.txt b/prbt_moveit_config/CMakeLists.txt index feaef4cf2..a6da4fa9c 100644 --- a/prbt_moveit_config/CMakeLists.txt +++ b/prbt_moveit_config/CMakeLists.txt @@ -1,6 +1,12 @@ cmake_minimum_required(VERSION 2.8.3) project(prbt_moveit_config) +add_compile_options(-Wall) +add_compile_options(-Wextra) +add_compile_options(-Wno-unused-parameter) +add_compile_options(-Wno-unused-variable) +add_compile_options(-Werror) + find_package(catkin REQUIRED COMPONENTS) catkin_package() @@ -11,5 +17,38 @@ install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) if (CATKIN_ENABLE_TESTING) find_package(roslaunch REQUIRED) + find_package(Eigen3 REQUIRED) + find_package(moveit_core REQUIRED) + find_package(moveit_ros_planning REQUIRED) + find_package(pluginlib REQUIRED) + find_package(roscpp REQUIRED) + find_package(rostest REQUIRED) + + include_directories(SYSTEM ${EIGEN3_INCLUDE_DIRS}) + include_directories(${roscpp_INCLUDE_DIRS}) + + if (CATKIN_ENABLE_CLANG_TIDY) + find_program( + CLANG_TIDY_EXE + NAMES "clang-tidy" + DOC "Path to clang-tidy executable" + ) + if(NOT CLANG_TIDY_EXE) + message(FATAL_ERROR "clang-tidy not found.") + else() + message(STATUS "clang-tidy found: ${CLANG_TIDY_EXE}") + set(CMAKE_CXX_CLANG_TIDY "${CLANG_TIDY_EXE}") + endif() + endif() + + add_rostest_gtest(unittest_prbt_ikfast_manipulator_plugin + test/tst_prbt_ikfast_manipulator_plugin.test + test/tst_prbt_ikfast_manipulator_plugin.cpp + ) + target_link_libraries(unittest_prbt_ikfast_manipulator_plugin + ${catkin_LIBRARIES} + ${moveit_ros_planning_LIBRARIES} + ) + roslaunch_add_file_check(launch/moveit_planning_execution.launch) endif() diff --git a/prbt_moveit_config/package.xml b/prbt_moveit_config/package.xml index d6285b6b3..b8e163bdf 100644 --- a/prbt_moveit_config/package.xml +++ b/prbt_moveit_config/package.xml @@ -34,5 +34,11 @@ prbt_hardware_support roslaunch + moveit_core + moveit_ros_planning + pluginlib + roscpp + rostest + rosunit diff --git a/prbt_ikfast_manipulator_plugin/test/unit_tests/tst_prbt_ikfast_manipulator_plugin.cpp b/prbt_moveit_config/test/tst_prbt_ikfast_manipulator_plugin.cpp similarity index 100% rename from prbt_ikfast_manipulator_plugin/test/unit_tests/tst_prbt_ikfast_manipulator_plugin.cpp rename to prbt_moveit_config/test/tst_prbt_ikfast_manipulator_plugin.cpp diff --git a/prbt_ikfast_manipulator_plugin/test/unit_tests/tst_prbt_ikfast_manipulator_plugin.test b/prbt_moveit_config/test/tst_prbt_ikfast_manipulator_plugin.test similarity index 75% rename from prbt_ikfast_manipulator_plugin/test/unit_tests/tst_prbt_ikfast_manipulator_plugin.test rename to prbt_moveit_config/test/tst_prbt_ikfast_manipulator_plugin.test index c419b5a4b..0fc7d7560 100644 --- a/prbt_ikfast_manipulator_plugin/test/unit_tests/tst_prbt_ikfast_manipulator_plugin.test +++ b/prbt_moveit_config/test/tst_prbt_ikfast_manipulator_plugin.test @@ -5,14 +5,14 @@ - + [prbt_joint_1, prbt_joint_2, prbt_joint_3, prbt_joint_4, prbt_joint_5, prbt_joint_6] - From c8020d40da7f0593d7fdd49201e53bdc2969eb34 Mon Sep 17 00:00:00 2001 From: Immanuel Martini Date: Tue, 4 Aug 2020 09:21:40 +0200 Subject: [PATCH 2/3] Apply clang-format --- .../tst_prbt_ikfast_manipulator_plugin.cpp | 96 +++++++++---------- 1 file changed, 44 insertions(+), 52 deletions(-) diff --git a/prbt_moveit_config/test/tst_prbt_ikfast_manipulator_plugin.cpp b/prbt_moveit_config/test/tst_prbt_ikfast_manipulator_plugin.cpp index 21663f8e8..61a98f6ff 100644 --- a/prbt_moveit_config/test/tst_prbt_ikfast_manipulator_plugin.cpp +++ b/prbt_moveit_config/test/tst_prbt_ikfast_manipulator_plugin.cpp @@ -10,27 +10,27 @@ typedef std::vector Vec1D; typedef std::vector Vec2D; typedef pluginlib::ClassLoader KinematicsLoader; -static const std::string MOVEIT_CORE_PACKAGE {"moveit_core"}; -static const std::string KINEMATICS_BASE_CLASS {"kinematics::KinematicsBase"}; +static const std::string MOVEIT_CORE_PACKAGE{ "moveit_core" }; +static const std::string KINEMATICS_BASE_CLASS{ "kinematics::KinematicsBase" }; -static const std::string PLUGIN_NAME_PARAM {"ik_plugin_name"}; -static const std::string GROUP_PARAM {"group"}; -static const std::string TIP_LINK_PARAM {"tip_link"}; -static const std::string ROOT_LINK_PARAM {"root_link"}; -static const std::string JOINT_NAMES_PARAM {"joint_names"}; -static const std::string ROBOT_DESCRIPTION_PARAM {"robot_description"}; +static const std::string PLUGIN_NAME_PARAM{ "ik_plugin_name" }; +static const std::string GROUP_PARAM{ "group" }; +static const std::string TIP_LINK_PARAM{ "tip_link" }; +static const std::string ROOT_LINK_PARAM{ "root_link" }; +static const std::string JOINT_NAMES_PARAM{ "joint_names" }; +static const std::string ROBOT_DESCRIPTION_PARAM{ "robot_description" }; -static constexpr double DEFAULT_SEARCH_DISCRETIZATION {0.01}; -static constexpr unsigned int NUM_JOINTS {6}; -static constexpr double IKFAST_TOLERANCE {0.001}; +static constexpr double DEFAULT_SEARCH_DISCRETIZATION{ 0.01 }; +static constexpr unsigned int NUM_JOINTS{ 6 }; +static constexpr double IKFAST_TOLERANCE{ 0.001 }; -static constexpr double L1 {0.3500}; // Height of first connector -static constexpr double L2 {0.3070}; // Height of second connector +static constexpr double L1{ 0.3500 }; // Height of first connector +static constexpr double L2{ 0.3070 }; // Height of second connector /** * @brief Overload ostream operator for double vectors. */ -std::ostream& operator<< (std::ostream &os, const Vec1D &vec) +std::ostream& operator<<(std::ostream& os, const Vec1D& vec) { os << "("; if (!vec.empty()) @@ -49,14 +49,11 @@ std::ostream& operator<< (std::ostream &os, const Vec1D &vec) /** * @brief Overload ostream operator for robot poses. */ -std::ostream& operator<< (std::ostream &os, const geometry_msgs::Pose &pose) +std::ostream& operator<<(std::ostream& os, const geometry_msgs::Pose& pose) { - os << "position: x=" << double(pose.position.x) - << ", y=" << double(pose.position.y) - << ", z=" << double(pose.position.z) - << ", orientation: x=" << double(pose.orientation.x) - << ", y=" << double(pose.orientation.y) - << ", z=" << double(pose.orientation.z) + os << "position: x=" << double(pose.position.x) << ", y=" << double(pose.position.y) + << ", z=" << double(pose.position.z) << ", orientation: x=" << double(pose.orientation.x) + << ", y=" << double(pose.orientation.y) << ", z=" << double(pose.orientation.z) << ", w=" << double(pose.orientation.w); return os; } @@ -66,7 +63,7 @@ std::ostream& operator<< (std::ostream &os, const geometry_msgs::Pose &pose) */ double singularPoseJointFormula(double joint1) { - return ( asin( L1 / L2 * sin(joint1) ) + joint1 ); + return (asin(L1 / L2 * sin(joint1)) + joint1); } /** @@ -90,8 +87,8 @@ class PrbtIKFastPluginTest : public ::testing::Test boost::shared_ptr solver_; private: - ros::NodeHandle ph_ {"~"}; - KinematicsLoader loader_ {MOVEIT_CORE_PACKAGE, KINEMATICS_BASE_CLASS}; + ros::NodeHandle ph_{ "~" }; + KinematicsLoader loader_{ MOVEIT_CORE_PACKAGE, KINEMATICS_BASE_CLASS }; }; void PrbtIKFastPluginTest::SetUp() @@ -105,27 +102,24 @@ void PrbtIKFastPluginTest::SetUp() { solver_ = loader_.createInstance(plugin_name); } - catch (pluginlib::PluginlibException &e) + catch (pluginlib::PluginlibException& e) { FAIL() << "Failed to load plugin: " << e.what(); } // initialize plugin - EXPECT_TRUE(ph_.getParam(GROUP_PARAM, group_name_)) - << "The parameter " << GROUP_PARAM << " was not found."; - EXPECT_TRUE(ph_.getParam(TIP_LINK_PARAM, tip_link_)) - << "The parameter " << TIP_LINK_PARAM << " was not found."; - EXPECT_TRUE(ph_.getParam(ROOT_LINK_PARAM, root_link_)) - << "The parameter " << ROOT_LINK_PARAM << " was not round."; + EXPECT_TRUE(ph_.getParam(GROUP_PARAM, group_name_)) << "The parameter " << GROUP_PARAM << " was not found."; + EXPECT_TRUE(ph_.getParam(TIP_LINK_PARAM, tip_link_)) << "The parameter " << TIP_LINK_PARAM << " was not found."; + EXPECT_TRUE(ph_.getParam(ROOT_LINK_PARAM, root_link_)) << "The parameter " << ROOT_LINK_PARAM << " was not round."; EXPECT_TRUE(ph_.getParam(JOINT_NAMES_PARAM, joint_names_)) << "The parameter " << JOINT_NAMES_PARAM << " was not found."; - std::vector tip_links {tip_link_}; + std::vector tip_links{ tip_link_ }; robot_model_loader::RobotModelLoader robot_model_loader(ROBOT_DESCRIPTION_PARAM, false); const robot_model::RobotModelPtr& robot_model = robot_model_loader.getModel(); - ASSERT_TRUE( solver_->initialize(*robot_model, group_name_, root_link_, tip_links, - DEFAULT_SEARCH_DISCRETIZATION) ) << "Failed to initialize plugin."; + ASSERT_TRUE(solver_->initialize(*robot_model, group_name_, root_link_, tip_links, DEFAULT_SEARCH_DISCRETIZATION)) + << "Failed to initialize plugin."; } void PrbtIKFastPluginTest::TearDown() @@ -154,22 +148,22 @@ TEST_F(PrbtIKFastPluginTest, testSingularities) // + Step 1 + // ++++++++++ - std::vector link_names { {solver_->getTipFrame()} }; + std::vector link_names{ { solver_->getTipFrame() } }; Vec1D seed; seed.resize(NUM_JOINTS, 0.); Vec2D joints_test_set; - joints_test_set.push_back(Vec1D { {0.0, 0.0, 0.0, 0.0, 0.0, 0.0} }); - joints_test_set.push_back(Vec1D { {0.0, 0.0, 0.0, 1.0, 0.0, 0.0} }); - joints_test_set.push_back(Vec1D { {0.0, 0.0, 0.0, 0.0, -1.0, 0.0} }); - joints_test_set.push_back(Vec1D { {0.0, 0.0, 0.0, 0.0, 1.0, 1.0} }); - - double j1 { 1.0 }; - double j2 { singularPoseJointFormula(j1) }; - joints_test_set.push_back(Vec1D { {0.0, j1, j2, 0.0, 0.0, 0.0} }); - joints_test_set.push_back(Vec1D { {0.0, j1, j2, -1.0, 0.0, 0.0} }); - joints_test_set.push_back(Vec1D { {0.0, j1, j2, 0.0, 1.0, 0.0} }); - joints_test_set.push_back(Vec1D { {0.0, j1, j2, 0.0, 1.0, -1.0} }); + joints_test_set.push_back(Vec1D{ { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } }); + joints_test_set.push_back(Vec1D{ { 0.0, 0.0, 0.0, 1.0, 0.0, 0.0 } }); + joints_test_set.push_back(Vec1D{ { 0.0, 0.0, 0.0, 0.0, -1.0, 0.0 } }); + joints_test_set.push_back(Vec1D{ { 0.0, 0.0, 0.0, 0.0, 1.0, 1.0 } }); + + double j1{ 1.0 }; + double j2{ singularPoseJointFormula(j1) }; + joints_test_set.push_back(Vec1D{ { 0.0, j1, j2, 0.0, 0.0, 0.0 } }); + joints_test_set.push_back(Vec1D{ { 0.0, j1, j2, -1.0, 0.0, 0.0 } }); + joints_test_set.push_back(Vec1D{ { 0.0, j1, j2, 0.0, 1.0, 0.0 } }); + joints_test_set.push_back(Vec1D{ { 0.0, j1, j2, 0.0, 1.0, -1.0 } }); // Same procedure for all joint vectors for (Vec1D joints : joints_test_set) @@ -181,8 +175,7 @@ TEST_F(PrbtIKFastPluginTest, testSingularities) // ++++++++++ std::vector poses; - ASSERT_TRUE( solver_->getPositionFK(link_names, joints, poses) ) - << "Failed to compute forward kinematics."; + ASSERT_TRUE(solver_->getPositionFK(link_names, joints, poses)) << "Failed to compute forward kinematics."; EXPECT_EQ(1u, poses.size()); ROS_INFO_STREAM("Obtain pose: " << poses.at(0)); @@ -195,12 +188,11 @@ TEST_F(PrbtIKFastPluginTest, testSingularities) kinematics::KinematicsQueryOptions options; ros::Time generation_begin = ros::Time::now(); - EXPECT_TRUE( solver_->getPositionIK(poses, seed, solutions, result, options) ) + EXPECT_TRUE(solver_->getPositionIK(poses, seed, solutions, result, options)) << "Failed to solve inverse kinematics."; double duration_ms = (ros::Time::now() - generation_begin).toSec() * 1000; ROS_DEBUG_STREAM("Ik solve took " << duration_ms << " ms"); - ROS_INFO_STREAM("Received " << solutions.size() << " solutions to IK problem."); EXPECT_GT(solutions.size(), 0u); @@ -208,12 +200,12 @@ TEST_F(PrbtIKFastPluginTest, testSingularities) // + Step 4 + // ++++++++++ - bool found_expected_solution {false}; + bool found_expected_solution{ false }; for (Vec1D sol : solutions) { EXPECT_EQ(sol.size(), NUM_JOINTS); - double diff {0.0}; + double diff{ 0.0 }; for (unsigned int j = 0; j < NUM_JOINTS; j++) { diff += pow(sol[j] - joints[j], 2); From 5e552d20564b78b2e506821c62393cdc9a12c2be Mon Sep 17 00:00:00 2001 From: Giuseppe Sansone Date: Fri, 7 Aug 2020 10:21:19 +0200 Subject: [PATCH 3/3] Add 'integrationtest_' prefix to ikfast_manipulator_plugin test. --- prbt_moveit_config/CMakeLists.txt | 8 ++++---- ...=> integrationtest_prbt_ikfast_manipulator_plugin.cpp} | 0 ...> integrationtest_prbt_ikfast_manipulator_plugin.test} | 4 ++-- 3 files changed, 6 insertions(+), 6 deletions(-) rename prbt_moveit_config/test/{tst_prbt_ikfast_manipulator_plugin.cpp => integrationtest_prbt_ikfast_manipulator_plugin.cpp} (100%) rename prbt_moveit_config/test/{tst_prbt_ikfast_manipulator_plugin.test => integrationtest_prbt_ikfast_manipulator_plugin.test} (74%) diff --git a/prbt_moveit_config/CMakeLists.txt b/prbt_moveit_config/CMakeLists.txt index a6da4fa9c..9ad145786 100644 --- a/prbt_moveit_config/CMakeLists.txt +++ b/prbt_moveit_config/CMakeLists.txt @@ -41,11 +41,11 @@ if (CATKIN_ENABLE_TESTING) endif() endif() - add_rostest_gtest(unittest_prbt_ikfast_manipulator_plugin - test/tst_prbt_ikfast_manipulator_plugin.test - test/tst_prbt_ikfast_manipulator_plugin.cpp + add_rostest_gtest(integrationtest_prbt_ikfast_manipulator_plugin + test/integrationtest_prbt_ikfast_manipulator_plugin.test + test/integrationtest_prbt_ikfast_manipulator_plugin.cpp ) - target_link_libraries(unittest_prbt_ikfast_manipulator_plugin + target_link_libraries(integrationtest_prbt_ikfast_manipulator_plugin ${catkin_LIBRARIES} ${moveit_ros_planning_LIBRARIES} ) diff --git a/prbt_moveit_config/test/tst_prbt_ikfast_manipulator_plugin.cpp b/prbt_moveit_config/test/integrationtest_prbt_ikfast_manipulator_plugin.cpp similarity index 100% rename from prbt_moveit_config/test/tst_prbt_ikfast_manipulator_plugin.cpp rename to prbt_moveit_config/test/integrationtest_prbt_ikfast_manipulator_plugin.cpp diff --git a/prbt_moveit_config/test/tst_prbt_ikfast_manipulator_plugin.test b/prbt_moveit_config/test/integrationtest_prbt_ikfast_manipulator_plugin.test similarity index 74% rename from prbt_moveit_config/test/tst_prbt_ikfast_manipulator_plugin.test rename to prbt_moveit_config/test/integrationtest_prbt_ikfast_manipulator_plugin.test index 0fc7d7560..d9b0a49c5 100644 --- a/prbt_moveit_config/test/tst_prbt_ikfast_manipulator_plugin.test +++ b/prbt_moveit_config/test/integrationtest_prbt_ikfast_manipulator_plugin.test @@ -5,14 +5,14 @@ - + [prbt_joint_1, prbt_joint_2, prbt_joint_3, prbt_joint_4, prbt_joint_5, prbt_joint_6] -