Move ikfast plugin test to prbt_moveit_config

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()

prbt_ikfast_manipulator_plugin/package.xml

@@ -26,9 +26,4 @@
   <exec_depend>pluginlib</exec_depend>
   <exec_depend>roscpp</exec_depend>
   <exec_depend>tf2_kdl</exec_depend>
-  <!-- Test dependencies -->
-  <test_depend>moveit_ros_planning</test_depend>
-  <test_depend>rostest</test_depend>
-  <test_depend>rosunit</test_depend>
-  <test_depend>code_coverage</test_depend>
 </package>

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(integrationtest_prbt_ikfast_manipulator_plugin
+    test/integrationtest_prbt_ikfast_manipulator_plugin.test
+    test/integrationtest_prbt_ikfast_manipulator_plugin.cpp
+  )
+  target_link_libraries(integrationtest_prbt_ikfast_manipulator_plugin
+    ${catkin_LIBRARIES}
+    ${moveit_ros_planning_LIBRARIES}
+  )
+
   roslaunch_add_file_check(launch/moveit_planning_execution.launch)
 endif()

prbt_moveit_config/package.xml

@@ -34,5 +34,11 @@
   <exec_depend>prbt_hardware_support</exec_depend>
 
   <test_depend>roslaunch</test_depend>
+  <test_depend>moveit_core</test_depend>
+  <test_depend>moveit_ros_planning</test_depend>
+  <test_depend>pluginlib</test_depend>
+  <test_depend>roscpp</test_depend>
+  <test_depend>rostest</test_depend>
+  <test_depend>rosunit</test_depend>
 
 </package>

prbt_ikfast_manipulator_plugin/test/unit_tests/tst_prbt_ikfast_manipulator_plugin.cpp → prbt_moveit_config/test/integrationtest_prbt_ikfast_manipulator_plugin.cpp

@@ -10,27 +10,27 @@ typedef std::vector<double> Vec1D;
 typedef std::vector<Vec1D> Vec2D;
 typedef pluginlib::ClassLoader<kinematics::KinematicsBase> 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<kinematics::KinematicsBase> 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<std::string> tip_links {tip_link_};
+  std::vector<std::string> 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<std::string> link_names { {solver_->getTipFrame()} };
+  std::vector<std::string> 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<geometry_msgs::Pose> 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,25 +188,24 @@ 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);
 
     // ++++++++++
     // + 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);

prbt_ikfast_manipulator_plugin/test/unit_tests/tst_prbt_ikfast_manipulator_plugin.test → prbt_moveit_config/test/integrationtest_prbt_ikfast_manipulator_plugin.test

@@ -5,14 +5,14 @@
     <arg name="load_robot_description" value="true"/>
   </include>
 
-  <test unless="$(arg debug)" pkg="prbt_ikfast_manipulator_plugin" type="unittest_prbt_ikfast_manipulator_plugin" test-name="unittest_prbt_ikfast_manipulator_plugin">
+  <test unless="$(arg debug)" pkg="prbt_moveit_config" type="integrationtest_prbt_ikfast_manipulator_plugin" test-name="integrationtest_prbt_ikfast_manipulator_plugin">
     <param name="tip_link" value="prbt_flange" />
     <param name="root_link" value="prbt_base_link" />
     <param name="group" value="manipulator" />
     <param name="ik_plugin_name" value="prbt_manipulator/IKFastKinematicsPlugin" />
     <rosparam param="joint_names">[prbt_joint_1, prbt_joint_2, prbt_joint_3, prbt_joint_4, prbt_joint_5, prbt_joint_6]</rosparam>
   </test>
-  <test if="$(arg debug)" pkg="prbt_ikfast_manipulator_plugin" type="unittest_prbt_ikfast_manipulator_plugin" test-name="unittest_prbt_ikfast_manipulator_plugin"
+  <test if="$(arg debug)" pkg="prbt_moveit_config" type="integrationtest_prbt_ikfast_manipulator_plugin" test-name="integrationtest_prbt_ikfast_manipulator_plugin"
         launch-prefix="xterm -e gdb -args">
     <param name="tip_link" value="prbt_flange" />
     <param name="root_link" value="prbt_base_link" />