00001 #ifndef __POINTMATCHER_ROS_TRANSFORM_H 00002 #define __POINTMATCHER_ROS_TRANSFORM_H 00003 00004 #include "pointmatcher/PointMatcher.h" 00005 #include "nav_msgs/Odometry.h" 00006 #include "Eigen/Eigen" 00007 00008 namespace ros 00009 { 00010 struct Time; 00011 }; 00012 namespace tf 00013 { 00014 struct Transform; 00015 struct TransformListener; 00016 struct StampedTransform; 00017 }; 00018 00019 namespace PointMatcher_ros 00020 { 00021 // from tf/ROS 00022 00023 template<typename T> 00024 typename PointMatcher<T>::TransformationParameters transformListenerToEigenMatrix(const tf::TransformListener &listener, const std::string& target, const std::string& source, const ros::Time& stamp); 00025 00026 template<typename T> 00027 typename PointMatcher<T>::TransformationParameters odomMsgToEigenMatrix(const nav_msgs::Odometry& odom); 00028 00029 // to tf/ROS 00030 00031 template<typename T> 00032 nav_msgs::Odometry eigenMatrixToOdomMsg(const typename PointMatcher<T>::TransformationParameters& inTr, const std::string& frame_id, const ros::Time& stamp); 00033 00034 template<typename T> 00035 tf::Transform eigenMatrixToTransform(const typename PointMatcher<T>::TransformationParameters& inTr); 00036 00037 template<typename T> 00038 tf::StampedTransform eigenMatrixToStampedTransform(const typename PointMatcher<T>::TransformationParameters& inTr, const std::string& target, const std::string& source, const ros::Time& stamp); 00039 00040 // 2D / 3D transform 00041 00042 template<typename T> 00043 typename PointMatcher<T>::TransformationParameters eigenMatrixToDim(const typename PointMatcher<T>::TransformationParameters& matrix, int dimp1); 00044 00045 }; // PointMatcher_ros 00046 00047 #endif //__POINTMATCHER_ROS_TRANSFORM_H