Port moveit_core/utils to ROS 2 (#68)

* Port moveit_core/utils to ROS 2 * moveit_core/utils adapt to logging #21 * moveit_core utils - xmlrpc_casts.cpp will not be used anymore since XML-RPC is not supported in ROS 2 * moveit_core utils - fixing CmakeLists.txt * moveit_core utils - restoring addLinkCollision headers
moveit · May 21, 2019 · 238ba19 · 238ba19
1 parent fa61645
commit 238ba19
Show file tree

Hide file tree

Showing 6 changed files with 53 additions and 201 deletions.
diff --git a/moveit_core/utils/CMakeLists.txt b/moveit_core/utils/CMakeLists.txt
@@ -1,35 +1,43 @@
 set(MOVEIT_LIB_NAME moveit_utils)
 
-add_library(${MOVEIT_LIB_NAME}
+add_library(${MOVEIT_LIB_NAME} SHARED
   src/lexical_casts.cpp
-  src/xmlrpc_casts.cpp
 )
-target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES})
 
+include_directories(include)
+ament_target_dependencies(${MOVEIT_LIB_NAME} visualization_msgs)
 set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
 
 install(TARGETS ${MOVEIT_LIB_NAME}
-        LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-        ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
+        LIBRARY DESTINATION "lib"
+        ARCHIVE DESTINATION "lib")
 
-if(CATKIN_ENABLE_TESTING)
+if(BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
+  find_package(moveit_resources REQUIRED)
   set(MOVEIT_TEST_LIB_NAME moveit_test_utils)
   add_library(${MOVEIT_TEST_LIB_NAME}
     src/robot_model_test_utils.cpp)
 
-  add_dependencies(${MOVEIT_TEST_LIB_NAME} ${catkin_EXPORTED_TARGETS})
-
-  find_package(moveit_resources REQUIRED)
-  include_directories(${moveit_resources_INCLUDE_DIRS})
+	include_directories(${moveit_resources_INCLUDE_DIRS})
 
-  target_link_libraries(${MOVEIT_TEST_LIB_NAME} ${catkin_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${Boost_LIBRARIES})
+  target_link_libraries(${MOVEIT_TEST_LIB_NAME}
+    ${Boost_LIBRARIES}
+    ${urdf_LIBRARIES}
+    ${srdfdom_LIBRARIES}
+    ${urdfdom_LIBRARIES}
+    ${urdfdom_headers_LIBRARIES}
+  )
 
   set_target_properties(${MOVEIT_TEST_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
 
-  install(TARGETS ${MOVEIT_TEST_LIB_NAME}
-          LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-          ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
-endif()
+  ament_export_libraries(${MOVEIT_TEST_LIB_NAME} ${MOVEIT_LIB_NAME})
+  ament_export_include_directories(include)
 
+  install(TARGETS
+    ${MOVEIT_TEST_LIB_NAME}
+    LIBRARY DESTINATION "lib"
+    ARCHIVE DESTINATION "lib")
+endif()
 
-install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION})
+install(DIRECTORY include/ DESTINATION "include")
diff --git a/moveit_core/utils/include/moveit/utils/robot_model_test_utils.h b/moveit_core/utils/include/moveit/utils/robot_model_test_utils.h
@@ -47,7 +47,7 @@
 #include <boost/filesystem/path.hpp>
 #include <moveit_resources/config.h>
 #include <moveit/robot_model/robot_model.h>
-#include <geometry_msgs/Pose.h>
+#include <geometry_msgs/msg/pose.hpp>
 
 namespace moveit
 {
@@ -110,36 +110,36 @@ class RobotModelBuilder
    * origins will default to the identity transform
    */
   void addChain(const std::string& section, const std::string& type,
-                const std::vector<geometry_msgs::Pose>& joint_origins = {});
+                const std::vector<geometry_msgs::msg::Pose>& joint_origins = {});
 
   /** \brief Adds a collision mesh to a specific link.
    *  \param[in] link_name The name of the link to which the mesh will be added. Must already be in the builder
    *  \param[in] filename The path to the mesh file, e.g.
    * "package://moveit_resources/pr2_description/urdf/meshes/base_v0/base_L.stl"
    *  \param[in] origin The origin pose of this collision mesh relative to the link origin
    */
-  void addCollisionMesh(const std::string& link_name, const std::string& filename, geometry_msgs::Pose origin);
+  void addCollisionMesh(const std::string& link_name, const std::string& filename, geometry_msgs::msg::Pose origin);
   /** \brief Adds a collision box to a specific link.
    *  \param[in] link_name The name of the link to which the box will be added. Must already be in the builder.
    *  \param[in] size The dimensions of the box
    *  \param[in] origin The origin pose of this collision box relative to the link origin
    */
-  void addCollisionBox(const std::string& link_name, const std::vector<double>& dims, geometry_msgs::Pose origin);
+  void addCollisionBox(const std::string& link_name, const std::vector<double>& dims, geometry_msgs::msg::Pose origin);
 
   /** \brief Adds a visual box to a specific link.
    *  \param[in] link_name The name of the link to which the box will be added. Must already be in the builder.
    *  \param[in] size The dimensions of the box
    *  \param[in] origin The origin pose of this visual box relative to the link origin
    */
-  void addVisualBox(const std::string& link_name, const std::vector<double>& size, geometry_msgs::Pose origin);
+  void addVisualBox(const std::string& link_name, const std::vector<double>& size, geometry_msgs::msg::Pose origin);
 
   /**
    * Adds an inertial component to a link.
    * \param[in] link_name The name of the link for this inertial information
    * \param[in] mass The mass of the link
    * \param[in] origin The origin center pose of the center of mass of this link
    */
-  void addInertial(const std::string& link_name, double mass, geometry_msgs::Pose origin, double ixx, double ixy,
+  void addInertial(const std::string& link_name, double mass, geometry_msgs::msg::Pose origin, double ixx, double ixy,
                    double ixz, double iyy, double iyz, double izz);
 
   /** \} */

diff --git a/moveit_core/utils/include/moveit/utils/xmlrpc_casts.h b/moveit_core/utils/include/moveit/utils/xmlrpc_casts.h
diff --git a/moveit_core/utils/src/lexical_casts.cpp b/moveit_core/utils/src/lexical_casts.cpp
@@ -35,7 +35,6 @@
 /* Author: Simon Schmeisser */
 
 #include "moveit/utils/lexical_casts.h"
-
 #include <locale>
 #include <sstream>
 

diff --git a/moveit_core/utils/src/robot_model_test_utils.cpp b/moveit_core/utils/src/robot_model_test_utils.cpp
@@ -34,17 +34,17 @@
 
 /* Author: Bryce Willey */
 
-#include <ros/ros.h>
+#include "rclcpp/rclcpp.hpp"
 #include <boost/algorithm/string_regex.hpp>
 #include <boost/math/constants/constants.hpp>
-#include <geometry_msgs/Pose.h>
+#include <geometry_msgs/msg/pose.hpp>
 #include "moveit/utils/robot_model_test_utils.h"
 
 namespace moveit
 {
 namespace core
 {
-const std::string LOGNAME = "robot_model_builder";
+rclcpp::Logger LOGGER_UTILS = rclcpp::get_logger("moveit").get_child("robot_model_builder");
 
 moveit::core::RobotModelPtr loadTestingRobotModel(const std::string& robot_name)
 {
@@ -69,7 +69,7 @@ urdf::ModelInterfaceSharedPtr loadModelInterface(const std::string& robot_name)
   urdf::ModelInterfaceSharedPtr urdf_model = urdf::parseURDFFile(urdf_path);
   if (urdf_model == nullptr)
   {
-    ROS_ERROR_NAMED(LOGNAME, "Cannot find URDF for %s. Make sure moveit_resources/your robot description is installed",
+    RCLCPP_ERROR(LOGGER_UTILS, "Cannot find URDF for %s. Make sure moveit_resources/your robot description is installed",
                     robot_name.c_str());
   }
   return urdf_model;
@@ -107,27 +107,27 @@ RobotModelBuilder::RobotModelBuilder(const std::string& name, const std::string&
 }
 
 void RobotModelBuilder::addChain(const std::string& section, const std::string& type,
-                                 const std::vector<geometry_msgs::Pose>& joint_origins)
+                                 const std::vector<geometry_msgs::msg::Pose>& joint_origins)
 {
   std::vector<std::string> link_names;
   boost::split_regex(link_names, section, boost::regex("->"));
   if (link_names.empty())
   {
-    ROS_ERROR_NAMED(LOGNAME, "No links specified (empty section?)");
+    RCLCPP_ERROR(LOGGER_UTILS, "No links specified (empty section?)");
     is_valid_ = false;
     return;
   }
   // First link should already be added.
   if (not urdf_model_->getLink(link_names[0]))
   {
-    ROS_ERROR_NAMED(LOGNAME, "Link %s not present in builder yet!", link_names[0].c_str());
+    RCLCPP_ERROR(LOGGER_UTILS, "Link %s not present in builder yet!", link_names[0].c_str());
     is_valid_ = false;
     return;
   }
 
   if (not joint_origins.empty() && link_names.size() - 1 != joint_origins.size())
   {
-    ROS_ERROR_NAMED(LOGNAME, "There should be one more link (%zu) than there are joint origins (%zu)",
+    RCLCPP_ERROR(LOGGER_UTILS, "There should be one more link (%zu) than there are joint origins (%zu)",
                     link_names.size(), joint_origins.size());
     is_valid_ = false;
     return;
@@ -139,7 +139,7 @@ void RobotModelBuilder::addChain(const std::string& section, const std::string&
     // These links shouldn't be present already.
     if (urdf_model_->getLink(link_names[i]))
     {
-      ROS_ERROR_NAMED(LOGNAME, "Link %s is already specified", link_names[i].c_str());
+      RCLCPP_ERROR(LOGGER_UTILS, "Link %s is already specified", link_names[i].c_str());
       is_valid_ = false;
       return;
     }
@@ -152,7 +152,7 @@ void RobotModelBuilder::addChain(const std::string& section, const std::string&
     joint->parent_to_joint_origin_transform.clear();
     if (not joint_origins.empty())
     {
-      geometry_msgs::Pose o = joint_origins[i - 1];
+      geometry_msgs::msg::Pose o = joint_origins[i - 1];
       joint->parent_to_joint_origin_transform.position = urdf::Vector3(o.position.x, o.position.y, o.position.z);
       joint->parent_to_joint_origin_transform.rotation =
           urdf::Rotation(o.orientation.x, o.orientation.y, o.orientation.z, o.orientation.w);
@@ -174,7 +174,7 @@ void RobotModelBuilder::addChain(const std::string& section, const std::string&
       joint->type = urdf::Joint::FIXED;
     else
     {
-      ROS_ERROR_NAMED(LOGNAME, "No such joint type as %s", type.c_str());
+      RCLCPP_ERROR(LOGGER_UTILS, "No such joint type as %s", type.c_str());
       is_valid_ = false;
       return;
     }
@@ -192,12 +192,12 @@ void RobotModelBuilder::addChain(const std::string& section, const std::string&
   }
 }
 
-void RobotModelBuilder::addInertial(const std::string& link_name, double mass, geometry_msgs::Pose origin, double ixx,
+void RobotModelBuilder::addInertial(const std::string& link_name, double mass, geometry_msgs::msg::Pose origin, double ixx,
                                     double ixy, double ixz, double iyy, double iyz, double izz)
 {
   if (not urdf_model_->getLink(link_name))
   {
-    ROS_ERROR_NAMED(LOGNAME, "Link %s not present in builder yet!", link_name.c_str());
+    RCLCPP_ERROR(LOGGER_UTILS, "Link %s not present in builder yet!", link_name.c_str());
     is_valid_ = false;
     return;
   }
@@ -220,7 +220,7 @@ void RobotModelBuilder::addInertial(const std::string& link_name, double mass, g
 }
 
 void RobotModelBuilder::addVisualBox(const std::string& link_name, const std::vector<double>& size,
-                                     geometry_msgs::Pose origin)
+                                     geometry_msgs::msg::Pose origin)
 {
   urdf::VisualSharedPtr vis(new urdf::Visual);
   urdf::BoxSharedPtr geometry(new urdf::Box);
@@ -230,11 +230,11 @@ void RobotModelBuilder::addVisualBox(const std::string& link_name, const std::ve
 }
 
 void RobotModelBuilder::addCollisionBox(const std::string& link_name, const std::vector<double>& dims,
-                                        geometry_msgs::Pose origin)
+                                        geometry_msgs::msg::Pose origin)
 {
   if (dims.size() != 3)
   {
-    ROS_ERROR("There can only be 3 dimensions of a box (given %zu!)", dims.size());
+    RCLCPP_ERROR(LOGGER_UTILS,"There can only be 3 dimensions of a box (given %zu!)");
     is_valid_ = false;
     return;
   }
@@ -246,7 +246,7 @@ void RobotModelBuilder::addCollisionBox(const std::string& link_name, const std:
 }
 
 void RobotModelBuilder::addCollisionMesh(const std::string& link_name, const std::string& filename,
-                                         geometry_msgs::Pose origin)
+                                         geometry_msgs::msg::Pose origin)
 {
   urdf::CollisionSharedPtr coll(new urdf::Collision);
   urdf::MeshSharedPtr geometry(new urdf::Mesh);
@@ -256,11 +256,11 @@ void RobotModelBuilder::addCollisionMesh(const std::string& link_name, const std
 }
 
 void RobotModelBuilder::addLinkCollision(const std::string& link_name, const urdf::CollisionSharedPtr& collision,
-                                         geometry_msgs::Pose origin)
+                                         geometry_msgs::msg::Pose origin)
 {
   if (not urdf_model_->getLink(link_name))
   {
-    ROS_ERROR_NAMED(LOGNAME, "Link %s not present in builder yet!", link_name.c_str());
+    RCLCPP_ERROR(LOGGER_UTILS, "Link %s not present in builder yet!", link_name.c_str());
     is_valid_ = false;
     return;
   }
@@ -274,11 +274,11 @@ void RobotModelBuilder::addLinkCollision(const std::string& link_name, const urd
 }
 
 void RobotModelBuilder::addLinkVisual(const std::string& link_name, const urdf::VisualSharedPtr& vis,
-                                      geometry_msgs::Pose origin)
+                                      geometry_msgs::msg::Pose origin)
 {
   if (not urdf_model_->getLink(link_name))
   {
-    ROS_ERROR_NAMED(LOGNAME, "Link %s not present in builder yet!", link_name.c_str());
+    RCLCPP_ERROR(LOGGER_UTILS, "Link %s not present in builder yet!", link_name.c_str());
     is_valid_ = false;
     return;
   }
@@ -357,7 +357,7 @@ moveit::core::RobotModelPtr RobotModelBuilder::build()
   }
   catch (urdf::ParseError& e)
   {
-    ROS_ERROR_NAMED(LOGNAME, "Failed to build tree: %s", e.what());
+    RCLCPP_ERROR(LOGGER_UTILS, "Failed to build tree: %s", e.what());
     return robot_model;
   }
 
@@ -368,7 +368,7 @@ moveit::core::RobotModelPtr RobotModelBuilder::build()
   }
   catch (urdf::ParseError& e)
   {
-    ROS_ERROR_NAMED(LOGNAME, "Failed to find root link: %s", e.what());
+    RCLCPP_ERROR(LOGGER_UTILS, "Failed to find root link: %s", e.what());
     return robot_model;
   }
   srdf_writer_->updateSRDFModel(*urdf_model_);