Port robot_model submodule of moveit_core to ROS 2

moveit_core/robot_model/CMakeLists.txt

@@ -1,5 +1,15 @@
+cmake_minimum_required(VERSION 3.5)
 set(MOVEIT_LIB_NAME moveit_robot_model)
 
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
 add_library(${MOVEIT_LIB_NAME}
   src/aabb.cpp
   src/fixed_joint_model.cpp
@@ -12,20 +22,24 @@ add_library(${MOVEIT_LIB_NAME}
   src/revolute_joint_model.cpp
   src/robot_model.cpp
   )
-set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
+# set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION ${${PROJECT_NAME}_VERSION})
 
-target_link_libraries(${MOVEIT_LIB_NAME} moveit_profiler moveit_exceptions moveit_kinematics_base ${catkin_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${Boost_LIBRARIES})
-add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS})
+target_link_libraries(${MOVEIT_LIB_NAME} moveit_profiler moveit_exceptions moveit_kinematics_base ${rclcpp_LIBRARIES} ${rmw_implementation_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${Boost_LIBRARIES})
+# add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS})
+ament_target_dependencies(${MOVEIT_LIB_NAME}
+  urdf
+  urdfdom_headers
+  visualization_msgs)
 
-if(CATKIN_ENABLE_TESTING)
+if(AMENT_ENABLE_TESTING)
   find_package(moveit_resources REQUIRED)
   include_directories(${moveit_resources_INCLUDE_DIRS})
 
   catkin_add_gtest(test_robot_model test/test.cpp)
-  target_link_libraries(test_robot_model moveit_test_utils ${catkin_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${MOVEIT_LIB_NAME})
+  target_link_libraries(test_robot_model moveit_test_utils ${rclcpp_LIBRARIES} ${rmw_implementation_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${MOVEIT_LIB_NAME})
 endif()
 
 install(TARGETS ${MOVEIT_LIB_NAME}
-        LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-        ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
-install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION})
+        LIBRARY DESTINATION lib
+        ARCHIVE DESTINATION lib)
+install(DIRECTORY include/ DESTINATION include)

moveit_core/robot_model/include/moveit/robot_model/joint_model.h

@@ -42,9 +42,14 @@
 #include <vector>
 #include <map>
 #include <iostream>
-#include <moveit_msgs/JointLimits.h>
+// #include <moveit_msgs/JointLimits.h>
+#include <moveit_msgs/msg/joint_limits.hpp>
 #include <random_numbers/random_numbers.h>
 #include <Eigen/Geometry>
+#include <rcutils/logging_macros.h>
+
+#define ROS_DEBUG_NAMED RCUTILS_LOG_ERROR
+#define ROS_WARN_NAMED RCUTILS_LOG_ERROR
 
 namespace moveit
 {
@@ -336,10 +341,10 @@ class JointModel
   void setVariableBounds(const std::string& variable, const VariableBounds& bounds);
 
   /** \brief Override joint limits loaded from URDF. Unknown variables are ignored. */
-  void setVariableBounds(const std::vector<moveit_msgs::JointLimits>& jlim);
+  void setVariableBounds(const std::vector<moveit_msgs::msg::JointLimits>& jlim);
 
   /** \brief Get the joint limits known to this model, as a message. */
-  const std::vector<moveit_msgs::JointLimits>& getVariableBoundsMsg() const
+  const std::vector<moveit_msgs::msg::JointLimits>& getVariableBoundsMsg() const
   {
     return variable_bounds_msg_;
   }
@@ -470,7 +475,7 @@ class JointModel
   /** \brief The bounds for each variable (low, high) in the same order as variable_names_ */
   Bounds variable_bounds_;
 
-  std::vector<moveit_msgs::JointLimits> variable_bounds_msg_;
+  std::vector<moveit_msgs::msg::JointLimits> variable_bounds_msg_;
 
   /** \brief Map from variable names to the corresponding index in variable_names_ (indexing makes sense within the
    * JointModel only) */

moveit_core/robot_model/include/moveit/robot_model/robot_model.h

@@ -40,7 +40,6 @@
 
 #include <moveit/macros/class_forward.h>
 #include <moveit/exceptions/exceptions.h>
-#include <urdf/model.h>
 #include <srdfdom/model.h>
 
 // joint types
@@ -53,6 +52,7 @@
 
 #include <Eigen/Geometry>
 #include <iostream>
+#include <moveit/logging/logging.h>
 
 /** \brief Main namespace for MoveIt! */
 namespace moveit

moveit_core/robot_model/src/floating_joint_model.cpp

@@ -37,7 +37,7 @@
 
 #include <moveit/robot_model/floating_joint_model.h>
 #include <boost/math/constants/constants.hpp>
-#include <ros/console.h>
+// #include <ros/console.h>
 #include <limits>
 #include <cmath>
 

moveit_core/robot_model/src/joint_model.cpp

@@ -133,7 +133,7 @@ void JointModel::setVariableBounds(const std::string& variable, const VariableBo
   computeVariableBoundsMsg();
 }
 
-void JointModel::setVariableBounds(const std::vector<moveit_msgs::JointLimits>& jlim)
+void JointModel::setVariableBounds(const std::vector<moveit_msgs::msg::JointLimits>& jlim)
 {
   for (std::size_t j = 0; j < variable_names_.size(); ++j)
     for (std::size_t i = 0; i < jlim.size(); ++i)
@@ -167,7 +167,7 @@ void JointModel::computeVariableBoundsMsg()
   variable_bounds_msg_.clear();
   for (std::size_t i = 0; i < variable_bounds_.size(); ++i)
   {
-    moveit_msgs::JointLimits lim;
+    moveit_msgs::msg::JointLimits lim;
     lim.joint_name = variable_names_[i];
     lim.has_position_limits = variable_bounds_[i].position_bounded_;
     lim.min_position = variable_bounds_[i].min_position_;