From 3a140802d537390f43e5ab017fc00ce4fb041299 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?V=C3=ADctor=20Mayoral=20Vilches?= Date: Tue, 19 Feb 2019 10:12:06 +0100 Subject: [PATCH 1/5] Port transforms submodule of moveit_core of ROS 2 --- moveit_core/transforms/CMakeLists.txt | 13 ++++++---- .../include/moveit/transforms/transforms.h | 25 ++++++++++--------- moveit_core/transforms/src/transforms.cpp | 18 ++++++------- 3 files changed, 30 insertions(+), 26 deletions(-) diff --git a/moveit_core/transforms/CMakeLists.txt b/moveit_core/transforms/CMakeLists.txt index 411b925a68..9dbb88184b 100644 --- a/moveit_core/transforms/CMakeLists.txt +++ b/moveit_core/transforms/CMakeLists.txt @@ -3,13 +3,16 @@ 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}) +# add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS}) +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) diff --git a/moveit_core/transforms/include/moveit/transforms/transforms.h b/moveit_core/transforms/include/moveit/transforms/transforms.h index 949a61341f..41b24fd2a1 100644 --- a/moveit_core/transforms/include/moveit/transforms/transforms.h +++ b/moveit_core/transforms/include/moveit/transforms/transforms.h @@ -37,11 +37,12 @@ #ifndef MOVEIT_TRANSFORMS_ #define MOVEIT_TRANSFORMS_ -#include -#include +#include +#include #include #include #include +#include namespace moveit { @@ -51,8 +52,8 @@ MOVEIT_CLASS_FORWARD(Transforms); /// @brief Map frame names to the transformation matrix that can transform objects from the frame name to the planning /// frame -typedef std::map, - Eigen::aligned_allocator > > +typedef std::map, + Eigen::aligned_allocator > > FixedTransformsMap; /** @brief Provides an implementation of a snapshot of a transform tree that can be easily queried for @@ -87,7 +88,7 @@ class Transforms : private boost::noncopyable /** * @brief Return all the transforms - * @return A map from string names of frames to corresponding Eigen::Isometry3d (w.r.t the planning frame) + * @return A map from string names of frames to corresponding Eigen::Affine3d (w.r.t the planning frame) */ const FixedTransformsMap& getAllTransforms() const; @@ -95,29 +96,29 @@ 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& transforms) const; + void copyTransforms(std::vector& transforms) const; /** * @brief Set a transform in the transform tree (adding it if necessary) * @param t The input transform (w.r.t the target frame) * @param from_frame The frame for which the input transform is specified */ - void setTransform(const Eigen::Isometry3d& t, const std::string& from_frame); + void setTransform(const Eigen::Affine3d& t, const std::string& from_frame); /** * @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& transforms); + void setTransforms(const std::vector& transforms); /** - * @brief Set all the transforms: a map from string names of frames to corresponding Eigen::Isometry3d (w.r.t the + * @brief Set all the transforms: a map from string names of frames to corresponding Eigen::Affine3d (w.r.t the * planning frame) */ void setAllTransforms(const FixedTransformsMap& transforms); @@ -170,7 +171,7 @@ class Transforms : private boost::noncopyable * @param t_in The input pose (in from_frame) * @param t_out The resultant (transformed) pose */ - void transformPose(const std::string& from_frame, const Eigen::Isometry3d& t_in, Eigen::Isometry3d& t_out) const + void transformPose(const std::string& from_frame, const Eigen::Affine3d& t_in, Eigen::Affine3d& t_out) const { t_out = getTransform(from_frame) * t_in; } @@ -192,7 +193,7 @@ class Transforms : private boost::noncopyable * @param from_frame The string id of the frame for which the transform is being computed * @return The required transform */ - virtual const Eigen::Isometry3d& getTransform(const std::string& from_frame) const; + virtual const Eigen::Affine3d& getTransform(const std::string& from_frame) const; protected: std::string target_frame_; diff --git a/moveit_core/transforms/src/transforms.cpp b/moveit_core/transforms/src/transforms.cpp index 8d086cce05..b1355e4dc2 100644 --- a/moveit_core/transforms/src/transforms.cpp +++ b/moveit_core/transforms/src/transforms.cpp @@ -37,7 +37,7 @@ #include #include #include -#include +#include namespace moveit { @@ -50,7 +50,7 @@ Transforms::Transforms(const std::string& target_frame) : target_frame_(target_f ROS_ERROR_NAMED("transforms", "The target frame for MoveIt! Transforms cannot be empty."); else { - transforms_map_[target_frame_] = Eigen::Isometry3d::Identity(); + transforms_map_[target_frame_] = Eigen::Affine3d::Identity(); } } @@ -86,7 +86,7 @@ bool Transforms::isFixedFrame(const std::string& frame) const return transforms_map_.find(frame) != transforms_map_.end(); } -const Eigen::Isometry3d& Transforms::getTransform(const std::string& from_frame) const +const Eigen::Affine3d& Transforms::getTransform(const std::string& from_frame) const { if (!from_frame.empty()) { @@ -100,7 +100,7 @@ const Eigen::Isometry3d& Transforms::getTransform(const std::string& from_frame) from_frame.c_str(), target_frame_.c_str()); // return identity - static const Eigen::Isometry3d IDENTITY = Eigen::Isometry3d::Identity(); + static const Eigen::Affine3d IDENTITY = Eigen::Affine3d::Identity(); return IDENTITY; } @@ -112,7 +112,7 @@ bool Transforms::canTransform(const std::string& from_frame) const return transforms_map_.find(from_frame) != transforms_map_.end(); } -void Transforms::setTransform(const Eigen::Isometry3d& t, const std::string& from_frame) +void Transforms::setTransform(const Eigen::Affine3d& t, const std::string& from_frame) { if (from_frame.empty()) ROS_ERROR_NAMED("transforms", "Cannot record transform with empty name"); @@ -120,11 +120,11 @@ void Transforms::setTransform(const Eigen::Isometry3d& t, const std::string& fro 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::Affine3d t = tf2::transformToEigen(transform); setTransform(t, transform.header.frame_id); } else @@ -134,13 +134,13 @@ void Transforms::setTransform(const geometry_msgs::TransformStamped& transform) } } -void Transforms::setTransforms(const std::vector& transforms) +void Transforms::setTransforms(const std::vector& transforms) { for (std::size_t i = 0; i < transforms.size(); ++i) setTransform(transforms[i]); } -void Transforms::copyTransforms(std::vector& transforms) const +void Transforms::copyTransforms(std::vector& transforms) const { transforms.resize(transforms_map_.size()); std::size_t i = 0; From 57814c6c233aa4ee228ca0f4704733b51ff596b5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?V=C3=ADctor=20Mayoral=20Vilches?= Date: Sun, 24 Feb 2019 11:04:45 +0100 Subject: [PATCH 2/5] transforms submodule, add tests Added tests to moveit_core submodule --- moveit_core/transforms/CMakeLists.txt | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/moveit_core/transforms/CMakeLists.txt b/moveit_core/transforms/CMakeLists.txt index 9dbb88184b..1e733c2af3 100644 --- a/moveit_core/transforms/CMakeLists.txt +++ b/moveit_core/transforms/CMakeLists.txt @@ -15,7 +15,8 @@ install(TARGETS ${MOVEIT_LIB_NAME} 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() From 6ca40b1b228bb3a0b3181a53650c55c74065c044 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?V=C3=ADctor=20Mayoral=20Vilches?= Date: Sun, 24 Feb 2019 11:08:38 +0100 Subject: [PATCH 3/5] Cleanup transforms submodule meta-files --- moveit_core/transforms/CMakeLists.txt | 1 - 1 file changed, 1 deletion(-) diff --git a/moveit_core/transforms/CMakeLists.txt b/moveit_core/transforms/CMakeLists.txt index 1e733c2af3..075e5615af 100644 --- a/moveit_core/transforms/CMakeLists.txt +++ b/moveit_core/transforms/CMakeLists.txt @@ -4,7 +4,6 @@ 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} ${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} tf2_eigen) From 94ebbcc191db16d6e3fae760d2d8b8062517d53f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?V=C3=ADctor=20Mayoral=20Vilches?= Date: Sun, 3 Mar 2019 21:13:06 +0100 Subject: [PATCH 4/5] moveit_core, transforms submodule, back to Isometry3d Revert the changes in transform and use Isometry3d after changes in geometry2 https://github.com/ros2/geometry2/pull/93 --- .../include/moveit/transforms/transforms.h | 14 +++++++------- moveit_core/transforms/src/transforms.cpp | 10 +++++----- 2 files changed, 12 insertions(+), 12 deletions(-) diff --git a/moveit_core/transforms/include/moveit/transforms/transforms.h b/moveit_core/transforms/include/moveit/transforms/transforms.h index 41b24fd2a1..aff1286c2c 100644 --- a/moveit_core/transforms/include/moveit/transforms/transforms.h +++ b/moveit_core/transforms/include/moveit/transforms/transforms.h @@ -52,8 +52,8 @@ MOVEIT_CLASS_FORWARD(Transforms); /// @brief Map frame names to the transformation matrix that can transform objects from the frame name to the planning /// frame -typedef std::map, - Eigen::aligned_allocator > > +typedef std::map, + Eigen::aligned_allocator > > FixedTransformsMap; /** @brief Provides an implementation of a snapshot of a transform tree that can be easily queried for @@ -88,7 +88,7 @@ class Transforms : private boost::noncopyable /** * @brief Return all the transforms - * @return A map from string names of frames to corresponding Eigen::Affine3d (w.r.t the planning frame) + * @return A map from string names of frames to corresponding Eigen::Isometry3d (w.r.t the planning frame) */ const FixedTransformsMap& getAllTransforms() const; @@ -103,7 +103,7 @@ class Transforms : private boost::noncopyable * @param t The input transform (w.r.t the target frame) * @param from_frame The frame for which the input transform is specified */ - void setTransform(const Eigen::Affine3d& t, const std::string& from_frame); + void setTransform(const Eigen::Isometry3d& t, const std::string& from_frame); /** * @brief Set a transform in the transform tree (adding it if necessary) @@ -118,7 +118,7 @@ class Transforms : private boost::noncopyable void setTransforms(const std::vector& transforms); /** - * @brief Set all the transforms: a map from string names of frames to corresponding Eigen::Affine3d (w.r.t the + * @brief Set all the transforms: a map from string names of frames to corresponding Eigen::Isometry3d (w.r.t the * planning frame) */ void setAllTransforms(const FixedTransformsMap& transforms); @@ -171,7 +171,7 @@ class Transforms : private boost::noncopyable * @param t_in The input pose (in from_frame) * @param t_out The resultant (transformed) pose */ - void transformPose(const std::string& from_frame, const Eigen::Affine3d& t_in, Eigen::Affine3d& t_out) const + void transformPose(const std::string& from_frame, const Eigen::Isometry3d& t_in, Eigen::Isometry3d& t_out) const { t_out = getTransform(from_frame) * t_in; } @@ -193,7 +193,7 @@ class Transforms : private boost::noncopyable * @param from_frame The string id of the frame for which the transform is being computed * @return The required transform */ - virtual const Eigen::Affine3d& getTransform(const std::string& from_frame) const; + virtual const Eigen::Isometry3d& getTransform(const std::string& from_frame) const; protected: std::string target_frame_; diff --git a/moveit_core/transforms/src/transforms.cpp b/moveit_core/transforms/src/transforms.cpp index b1355e4dc2..dc914065c1 100644 --- a/moveit_core/transforms/src/transforms.cpp +++ b/moveit_core/transforms/src/transforms.cpp @@ -50,7 +50,7 @@ Transforms::Transforms(const std::string& target_frame) : target_frame_(target_f ROS_ERROR_NAMED("transforms", "The target frame for MoveIt! Transforms cannot be empty."); else { - transforms_map_[target_frame_] = Eigen::Affine3d::Identity(); + transforms_map_[target_frame_] = Eigen::Isometry3d::Identity(); } } @@ -86,7 +86,7 @@ bool Transforms::isFixedFrame(const std::string& frame) const return transforms_map_.find(frame) != transforms_map_.end(); } -const Eigen::Affine3d& Transforms::getTransform(const std::string& from_frame) const +const Eigen::Isometry3d& Transforms::getTransform(const std::string& from_frame) const { if (!from_frame.empty()) { @@ -100,7 +100,7 @@ const Eigen::Affine3d& Transforms::getTransform(const std::string& from_frame) c from_frame.c_str(), target_frame_.c_str()); // return identity - static const Eigen::Affine3d IDENTITY = Eigen::Affine3d::Identity(); + static const Eigen::Isometry3d IDENTITY = Eigen::Isometry3d::Identity(); return IDENTITY; } @@ -112,7 +112,7 @@ bool Transforms::canTransform(const std::string& from_frame) const return transforms_map_.find(from_frame) != transforms_map_.end(); } -void Transforms::setTransform(const Eigen::Affine3d& t, const std::string& from_frame) +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"); @@ -124,7 +124,7 @@ void Transforms::setTransform(const geometry_msgs::msg::TransformStamped& transf { if (sameFrame(transform.child_frame_id, target_frame_)) { - Eigen::Affine3d t = tf2::transformToEigen(transform); + Eigen::Isometry3d t = tf2::transformToEigen(transform); setTransform(t, transform.header.frame_id); } else From 7fa3dfa14d0a91fe1bec8afe8a791610a9bf5f7b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?V=C3=ADctor=20Mayoral=20Vilches?= Date: Mon, 4 Mar 2019 22:47:03 +0100 Subject: [PATCH 5/5] transforms, fix logging Follows from https://github.com/ros-planning/moveit2/issues/21 Changes in the code, particularly in the for loops are required due to the use of rclcpp macros --- moveit_core/transforms/src/transforms.cpp | 20 +++++++++++++------- 1 file changed, 13 insertions(+), 7 deletions(-) diff --git a/moveit_core/transforms/src/transforms.cpp b/moveit_core/transforms/src/transforms.cpp index dc914065c1..c5b4ad1255 100644 --- a/moveit_core/transforms/src/transforms.cpp +++ b/moveit_core/transforms/src/transforms.cpp @@ -37,17 +37,22 @@ #include #include #include -#include +#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,8 +119,9 @@ 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; } @@ -129,7 +135,7 @@ void Transforms::setTransform(const geometry_msgs::msg::TransformStamped& transf } 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()); } }