38 #include <geometric_shapes/check_isometry.h>
39 #include <rclcpp/logger.hpp>
40 #include <rclcpp/logging.hpp>
41 #include <tf2_eigen/tf2_eigen.hpp>
42 #include <boost/algorithm/string/trim.hpp>
49 static const rclcpp::Logger LOGGER =
rclcpp::get_logger(
"moveit_transforms.transforms");
56 RCLCPP_ERROR(LOGGER,
"The target frame for MoveIt Transforms cannot be empty.");
66 if (frame1.empty() || frame2.empty())
68 return frame1 == frame2;
85 for (
const auto& t : transforms)
87 ASSERT_ISOMETRY(t.second)
106 if (!from_frame.empty())
108 FixedTransformsMap::const_iterator it =
transforms_map_.find(from_frame);
114 RCLCPP_ERROR(LOGGER,
"Unable to transform from frame '%s' to frame '%s'. Returning identity.", from_frame.c_str(),
118 static const Eigen::Isometry3d IDENTITY = Eigen::Isometry3d::Identity();
124 if (from_frame.empty())
137 if (from_frame.empty())
139 RCLCPP_ERROR(LOGGER,
"Cannot record transform with empty name");
151 const auto& trans = transform.transform.translation;
152 const auto& rot = transform.transform.rotation;
153 Eigen::Translation3d translation(trans.x, trans.y, trans.z);
154 Eigen::Quaterniond rotation(rot.w, rot.x, rot.y, rot.z);
155 rotation.normalize();
157 setTransform(translation * rotation, transform.header.frame_id);
161 RCLCPP_ERROR(LOGGER,
"Given transform is to frame '%s', but frame '%s' was expected.",
168 for (
const geometry_msgs::msg::TransformStamped& transform : transforms)
178 transforms[i] = tf2::eigenToTransform(it->second);
180 transforms[i].header.frame_id = it->first;
std::map< std::string, Eigen::Isometry3d, std::less< std::string >, Eigen::aligned_allocator< std::pair< const std::string, Eigen::Isometry3d > > > FixedTransformsMap
Map frame names to the transformation matrix that can transform objects from the frame name to the pl...
rclcpp::Logger get_logger()
Main namespace for MoveIt.