Port moveit_core/kinematics_metrics (#66)

* kinematics_metrics, port to ROS 2 * kinematics_metrics, adapt logging to #21
moveit · Apr 11, 2019 · 7bbbb4b · 7bbbb4b
1 parent d132763
commit 7bbbb4b
Show file tree

Hide file tree

Showing 2 changed files with 21 additions and 11 deletions.
diff --git a/moveit_core/kinematics_metrics/CMakeLists.txt b/moveit_core/kinematics_metrics/CMakeLists.txt
@@ -3,11 +3,14 @@ set(MOVEIT_LIB_NAME moveit_kinematics_metrics)
 add_library(${MOVEIT_LIB_NAME} src/kinematics_metrics.cpp)
 set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
 
-target_link_libraries(${MOVEIT_LIB_NAME} moveit_robot_state ${catkin_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${Boost_LIBRARIES})
-add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS})
+ament_target_dependencies(${MOVEIT_LIB_NAME}
+  moveit_robot_state
+  urdf
+  urdfdom_headers
+  visualization_msgs)
 
 install(TARGETS ${MOVEIT_LIB_NAME}
-  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
+        LIBRARY DESTINATION lib
+        ARCHIVE DESTINATION lib)
 
-install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION})
+install(DIRECTORY include/ DESTINATION include)
diff --git a/moveit_core/kinematics_metrics/src/kinematics_metrics.cpp b/moveit_core/kinematics_metrics/src/kinematics_metrics.cpp
@@ -38,9 +38,13 @@
 #include <Eigen/Dense>
 #include <Eigen/Eigenvalues>
 #include <boost/math/constants/constants.hpp>
+#include "rclcpp/rclcpp.hpp"
 
 namespace kinematics_metrics
 {
+
+rclcpp::Logger LOGGER = rclcpp::get_logger("moveit").get_child("kinematics_metrics");
+
 double KinematicsMetrics::getJointLimitsPenalty(const robot_state::RobotState& state,
                                                 const robot_model::JointModelGroup* joint_model_group) const
 {
@@ -123,7 +127,7 @@ bool KinematicsMetrics::getManipulabilityIndex(const robot_state::RobotState& st
       manipulability_index = 1.0;
       for (unsigned int i = 0; i < singular_values.rows(); ++i)
       {
-        ROS_DEBUG_NAMED("kinematics_metrics", "Singular value: %d %f", i, singular_values(i, 0));
+        RCLCPP_DEBUG(LOGGER, "Singular value: %d %f", i, singular_values(i, 0));
         manipulability_index *= singular_values(i, 0);
       }
       // Get manipulability index
@@ -146,7 +150,7 @@ bool KinematicsMetrics::getManipulabilityIndex(const robot_state::RobotState& st
       manipulability_index = 1.0;
       for (unsigned int i = 0; i < singular_values.rows(); ++i)
       {
-        ROS_DEBUG_NAMED("kinematics_metrics", "Singular value: %d %f", i, singular_values(i, 0));
+        RCLCPP_DEBUG(LOGGER, "Singular value: %d %f", i, singular_values(i, 0));
         manipulability_index *= singular_values(i, 0);
       }
       // Get manipulability index
@@ -218,17 +222,20 @@ bool KinematicsMetrics::getManipulability(const robot_state::RobotState& state,
     Eigen::MatrixXd jacobian = state.getJacobian(joint_model_group);
     Eigen::JacobiSVD<Eigen::MatrixXd> svdsolver(jacobian.topLeftCorner(3, jacobian.cols()));
     Eigen::MatrixXd singular_values = svdsolver.singularValues();
-    for (int i = 0; i < singular_values.rows(); ++i)
-      ROS_DEBUG_NAMED("kinematics_metrics", "Singular value: %d %f", i, singular_values(i, 0));
+    for (int i = 0; i < singular_values.rows(); ++i){
+      RCLCPP_DEBUG(LOGGER, "Singular value: %d %f", i, singular_values(i, 0));
+    }
+
     manipulability = penalty * singular_values.minCoeff() / singular_values.maxCoeff();
   }
   else
   {
     Eigen::MatrixXd jacobian = state.getJacobian(joint_model_group);
     Eigen::JacobiSVD<Eigen::MatrixXd> svdsolver(jacobian);
     Eigen::MatrixXd singular_values = svdsolver.singularValues();
-    for (int i = 0; i < singular_values.rows(); ++i)
-      ROS_DEBUG_NAMED("kinematics_metrics", "Singular value: %d %f", i, singular_values(i, 0));
+    for (int i = 0; i < singular_values.rows(); ++i){
+          RCLCPP_DEBUG(LOGGER, "Singular value: %d %f", i, singular_values(i, 0));
+    }
     manipulability = penalty * singular_values.minCoeff() / singular_values.maxCoeff();
   }
   return true;