Port transforms submodule of moveit_core of ROS 2

moveit_core/transforms/CMakeLists.txt

@@ -3,16 +3,19 @@ set(MOVEIT_LIB_NAME moveit_transforms)
 add_library(${MOVEIT_LIB_NAME} src/transforms.cpp)
 set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
 
-target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${Boost_LIBRARIES})
-add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS})
+target_link_libraries(${MOVEIT_LIB_NAME} ${rclcpp_LIBRARIES} ${rmw_implementation_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${Boost_LIBRARIES})
+ament_target_dependencies(${MOVEIT_LIB_NAME}
+            tf2_eigen)
+
 
 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)
 
 # Unit tests
-if(CATKIN_ENABLE_TESTING)
-  catkin_add_gtest(test_transforms test/test_transforms.cpp)
+if(BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
+  ament_add_gtest(test_transforms test/test_transforms.cpp)
   target_link_libraries(test_transforms ${MOVEIT_LIB_NAME})
 endif()

moveit_core/transforms/include/moveit/transforms/transforms.h

@@ -37,11 +37,12 @@
 #ifndef MOVEIT_TRANSFORMS_
 #define MOVEIT_TRANSFORMS_
 
-#include <geometry_msgs/TransformStamped.h>
-#include <geometry_msgs/Pose.h>
+#include <geometry_msgs/msg/transform_stamped.hpp>
+#include <geometry_msgs/msg/pose.hpp>
 #include <Eigen/Geometry>
 #include <boost/noncopyable.hpp>
 #include <moveit/macros/class_forward.h>
+#include <map>
 
 namespace moveit
 {
@@ -95,7 +96,7 @@ class Transforms : private boost::noncopyable
    * @brief Get a vector of all the transforms as ROS messages
    * @param transforms The output transforms
    */
-  void copyTransforms(std::vector<geometry_msgs::TransformStamped>& transforms) const;
+  void copyTransforms(std::vector<geometry_msgs::msg::TransformStamped>& transforms) const;
 
   /**
    * @brief Set a transform in the transform tree (adding it if necessary)
@@ -108,13 +109,13 @@ class Transforms : private boost::noncopyable
    * @brief Set a transform in the transform tree (adding it if necessary)
    * @param transform The input transform (the frame_id must match the target frame)
    */
-  void setTransform(const geometry_msgs::TransformStamped& transform);
+  void setTransform(const geometry_msgs::msg::TransformStamped& transform);
 
   /**
    * @brief Set a transform in the transform tree (adding it if necessary)
    * @param transform The input transforms (the frame_id must match the target frame)
    */
-  void setTransforms(const std::vector<geometry_msgs::TransformStamped>& transforms);
+  void setTransforms(const std::vector<geometry_msgs::msg::TransformStamped>& transforms);
 
   /**
    * @brief Set all the transforms: a map from string names of frames to corresponding Eigen::Isometry3d (w.r.t the

moveit_core/transforms/src/transforms.cpp

@@ -37,17 +37,22 @@
 #include <moveit/transforms/transforms.h>
 #include <tf2_eigen/tf2_eigen.h>
 #include <boost/algorithm/string/trim.hpp>
-#include <ros/console.h>
+#include "rclcpp/rclcpp.hpp"
 
 namespace moveit
 {
 namespace core
 {
+
+// Logger
+rclcpp::Logger logger_transforms = rclcpp::get_logger("transforms");
+
 Transforms::Transforms(const std::string& target_frame) : target_frame_(target_frame)
 {
   boost::trim(target_frame_);
-  if (target_frame_.empty())
-    ROS_ERROR_NAMED("transforms", "The target frame for MoveIt! Transforms cannot be empty.");
+  if (target_frame_.empty()){
+    RCLCPP_ERROR(logger_transforms, "The target frame for MoveIt! Transforms cannot be empty.");
+  }
   else
   {
     transforms_map_[target_frame_] = Eigen::Isometry3d::Identity();
@@ -96,7 +101,7 @@ const Eigen::Isometry3d& Transforms::getTransform(const std::string& from_frame)
     // If no transform found in map, return identity
   }
 
-  ROS_ERROR_NAMED("transforms", "Unable to transform from frame '%s' to frame '%s'. Returning identity.",
+  RCLCPP_ERROR(logger_transforms, "Unable to transform from frame '%s' to frame '%s'. Returning identity.",
                   from_frame.c_str(), target_frame_.c_str());
 
   // return identity
@@ -114,33 +119,34 @@ bool Transforms::canTransform(const std::string& from_frame) const
 
 void Transforms::setTransform(const Eigen::Isometry3d& t, const std::string& from_frame)
 {
-  if (from_frame.empty())
-    ROS_ERROR_NAMED("transforms", "Cannot record transform with empty name");
+  if (from_frame.empty()){
+    RCLCPP_ERROR(logger_transforms, "Cannot record transform with empty name");
+  }
   else
     transforms_map_[from_frame] = t;
 }
 
-void Transforms::setTransform(const geometry_msgs::TransformStamped& transform)
+void Transforms::setTransform(const geometry_msgs::msg::TransformStamped& transform)
 {
   if (sameFrame(transform.child_frame_id, target_frame_))
   {
-    Eigen::Isometry3d t = tf2::transformToEigen(transform.transform);
+    Eigen::Isometry3d t = tf2::transformToEigen(transform);
     setTransform(t, transform.header.frame_id);
   }
   else
   {
-    ROS_ERROR_NAMED("transforms", "Given transform is to frame '%s', but frame '%s' was expected.",
+    RCLCPP_ERROR(logger_transforms, "Given transform is to frame '%s', but frame '%s' was expected.",
                     transform.child_frame_id.c_str(), target_frame_.c_str());
   }
 }
 
-void Transforms::setTransforms(const std::vector<geometry_msgs::TransformStamped>& transforms)
+void Transforms::setTransforms(const std::vector<geometry_msgs::msg::TransformStamped>& transforms)
 {
   for (std::size_t i = 0; i < transforms.size(); ++i)
     setTransform(transforms[i]);
 }
 
-void Transforms::copyTransforms(std::vector<geometry_msgs::TransformStamped>& transforms) const
+void Transforms::copyTransforms(std::vector<geometry_msgs::msg::TransformStamped>& transforms) const
 {
   transforms.resize(transforms_map_.size());
   std::size_t i = 0;