写ROS程序时会经常遇到tf与Eigen库的转换,即算法中大多会使用Eigen来进行运算和表示机器人的位姿,但是最终需要tf将pose发布出去,所以需要将Eigen表示的pose转换为tf以及相应的msgs。
首先,在“Transform.h”头文件中定义了Transform类,类成员函数 void mult(const Transform& t1, const Transform& t2) 可以很方便地用于机器人的坐标转换。在"transform_datatypes.h"头文件中又定义了StampedTransform类,该类继承自Transform类,新增加了时间戳以及frame_id:
/** \brief The Stamped Transform datatype used by tf */
class StampedTransform : public tf::Transform
{
public:
ros::Time stamp_; ///< The timestamp associated with this transform
std::string frame_id_; ///< The frame_id of the coordinate frame in which this transform is defined
std::string child_frame_id_; ///< The frame_id of the coordinate frame this transform defines
StampedTransform(const tf::Transform& input, const ros::Time& timestamp, const std::string & frame_id, const std::string & child_frame_id):
tf::Transform (input), stamp_ ( timestamp ), frame_id_ (frame_id), child_frame_id_(child_frame_id){ };
/** \brief Default constructor only to be used for preallocation */
StampedTransform() { };
/** \brief Set the inherited Traonsform data */
void setData(const tf::Transform& input){*static_cast(this) = input;};
};
ROS中对外发布tf的方式为调用“tf::TransformBroadcaster”类的"sendTransform()"函数,在头文件"transform_broadcaster.h”中定义:
/** \brief Send a StampedTransform
* The stamped data structure includes frame_id, and time, and parent_id already. */
void sendTransform(const StampedTransform & transform);
/** \brief Send a TransformStamped message
* The stamped data structure includes frame_id, and time, and parent_id already. */
void sendTransform(const geometry_msgs::TransformStamped & transform);
可见该函数不仅可以接收标准的 “geometry_msgs::TransformStamped” 数据,也可以接收"tf::StampedTransform"类型的数据,虽然在"transform_datatypes.h"中也定义了一个静态函数 “static void transformStampedTFToMsg(const StampedTransform& bt, geometry_msgs::TransformStamped & msg)” 用于将“StampedTransform”类型的数据转换为”geometry_msgs::TransformStamped“类型的数据。
假设我们使用激光传感器来做机器人定位, 算法给出的是激光传感器在地图坐标系中的位姿,即tf_map2scan,而我们实际需要的是车体在地图坐标系中位姿tf_map2base。又已知车体中心和激光传感器之间有一个固定的变换关系tf_base2scan,这个tf是由static_transform_publisher发布的,与激光传感器的安装位置有关,故程序中需要监听该Transform:
// 先获得base_link到scan的tf转换关系,以便发布map到base_link的tf
tf::StampedTransform tf_base2scan;
tf::TransformListener tf_listener;
tf_listener.waitForTransform("/base_link", "/scan", ros::Time(0), ros::Duration(1.0));
tf_listener.lookupTransform("/base_link", "/scan", ros::Time(0), tf_base2scan);
定位算法的结果是使用 ”Eigen::Matrix4f globalPose“ 这个Eigen的变换矩阵来表示,我们要做的是转换为tf来表示。首先将变换矩阵拆分为一个旋转矩阵(实际使用四元数表示)和一个平移矩阵:
Eigen::Quaterniond eigen_quat(globalPose.block<3,3>(0,0).cast());
Eigen::Vector3d eigen_trans(globalPose.block<3,1>(0,3).cast());
然后将其转换为相应的tf变量:
tf::Quaternion tf_quat;
tf::Vector3 tf_trans;
tf::quaternionEigenToTF(eigen_quat, tf_quat);
tf::vectorEigenToTF(eigen_trans, tf_trans);
”tf::quaternionEigenToTF“ 和 "tf::vectorEigenToTF" 函数在头文件
tf::Quaternion tf_quat(eigen_quat.x(), eigen_quat.y(), eigen_quat.z(), eigen_quat.w());
tf::Vector3 tf_trans(eigen_trans(0), eigen_trans(1), eigen_trans(2));
最后构建激光传感器与地图坐标系的tf变换:
tf::StampedTransform tf_map2scan(tf::Transform(tf_quat, tf_trans), current_time, "map", "scan");
得到激光传感器与地图坐标系的tf关系 ”tf_map2scan“ 以及激光传感器与车体坐标系的tf关系 ”tf_base2scan“ 以后,便可以计算地图坐标系到车体坐标系的tf,即 tf_map2base = tf_map2scan * tf_scan2base:
tf::StampedTransform tf_map2base;
tf_map2base.mult(tf_map2scan, tf_base2scan.inverse());
tf_map2base.stamp_ = current_time;
tf_map2base.frame_id_ = "map";
tf_map2base.child_frame_id_ = "base_link";
最后发布该tf关系:
broadcaster.sendTransform(tf_map2base);