1.数据类型
数据类型定义在tf/transform_datatypes.h.里
1.1 基本数据类型(Quaternion, Vector, Point, Pose, Transform)
| Type | tf |
|---|---|
| Quaternion | tf::Quaternion |
| Vector | tf::Vector3 |
| Point | tf::Point |
| Pose | tf::Pose |
| Transform | tf::Transform |
1.2 tf::Stamped
tf::Stamped在上面的数据类型(tf::Transform除外)上被模板化，元素有frame_id_和stamp_
template <typename T>
class Stamped : public T{
public:
ros::Time stamp_;
std::string frame_id_;
Stamped() :frame_id_ ("NO_ID_STAMPED_DEFAULT_CONSTRUCTION"){}; //仅用于预分配的默认构造函数
Stamped(const T& input, const ros::Time& timestamp, const std::string & frame_id);
void setData(const T& input);
};
1.3 tf::StampedTransform
tf::StampedTransform是tf::Transforms的一个特例，同时要求具有frame_id，stamp 和 child_frame_id。
/** \tf使用的stamp Transform数据类型*/
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<tf::Transform*>(this) = input;};
};
2. 辅助函数
tf::Quaternion createIdentityQuaternion()//返回一个四元数
tf::Quaternion createQuaternionFromRPY(double roll,double pitch,double yaw)
返回一个由由固定轴横滚角、俯仰角和偏航角组成的tf::Quaternion
geometry_msgs::Quaternion createQuaternionMsgFromRollPitchYaw(double roll,double pitch,double yaw)
返回一个由由固定轴横滚角、俯仰角和偏航角组成的geometry_msgs::Quaternion
3.TF使用方法
当我们使用TF包时，需要编写两个程序，分别用来执行监听TF变换和广播TF变换的功能，我们称它们为TF监听器和TF广播器。
- TF监听器：监听TF变换，接收并缓存系统中发布的所有参考系变换，并从中查询所需要的参考系变换。
- TF广播器：广播TF变换，向系统中广播参考系之间的坐标变换关系。系统中可能会存在多个不同部分的tf变换广播，但每个广播都可以直接将参考系变换关系直接插入tf树中，不需要再进行同步。
4. TransformBroadcaster
此类提供了一种发布坐标系变换信息的简便方法。它将处理所有消息和消息填充。函数原型列出了每条消息所需的所有必要数据。
void tf::TransformBroadcaster::sendTransform(const StampedTransform & transform)
发送StampedTransform，stamped数据结构包括frame_id、time和parent_id。
void tf::TransformBroadcaster::sendTransform(const std::vector< StampedTransform > & transforms)
发送一个vector的StampedTransform，stamped数据结构包括frame_id、time和parent_id。
void tf::TransformBroadcaster::sendTransform(const geometry_msgs::TransformStamped & transform)
发送一个StampedTransform message，stamped数据结构包括frame_id、time和parent_id。
void tf::TransformBroadcaster::sendTransform(const std::vector< geometry_msgs::TransformStamped > & transforms)
发送一个vector的StampedTransform message，stamped数据结构包括frame_id、time和parent_id。
测试代码
// 创建tf的广播器
static tf::TransformBroadcaster br;
// 初始化tf数据
tf::Transform transform;
transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );
tf::Quaternion q;
q.setRPY(0, 0, msg->theta);
transform.setRotation(q);
// 广播world与海龟坐标系之间的tf数据
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
运行结果
5. TransformListener
此类继承自Transformer，并自动订阅 ROS transform messages。
lookupTransform
函数功能：可以过得两个坐标系之间转换的关系，包括旋转与平移。
(1)这个lookupTransform()API，有六个参数：
- 变换从坐标系turtle2
- 到turtle1坐标系
- 在now时间
- 变换结果保存的变量
transformPoint
函数功能：这个在传感器数据的坐标变换中使用的比较多，用来将一个传感器的数据从一个坐标系转换到另外一个坐标系下。
waitForTransform
函数功能：等待变换在tf树中生效
(1) waitForTransform()有四个参数：
- turtle2，父类坐标系
- carrot1，子类坐标系
- rospy.Time()，这个时间的变换，就是变换的时刻
- rospy.Duration(4.0)，等待最长的时间段
(2) waitForTransform() 实际上会阻塞，阻塞时间由第四个参数决定，直到两个坐标系变换开始。（通常是几毫秒）
(3) waitForTransform()常常会调用两次，一开始产生turtle2时候 ，在第一次调用waitForTransform()前，TF可能还没有更新。第一次调用waitForTransform()就会等待，直到/turtle2坐标系开始广播。第二次调用waitForTransform()用Now参数才能获取到值。
transfromPose
函数功能：转换位姿，将某位姿转换到指定坐标系下的位姿表示。
测试代码
int main(int argc, char** argv)
{
// 初始化ROS节点
ros::init(argc, argv, "my_tf_listener");
// 创建节点句柄
ros::NodeHandle node;
// 请求产生turtle2
ros::service::waitForService("/spawn");
ros::ServiceClient add_turtle = node.serviceClient<turtlesim::Spawn>("/spawn");
turtlesim::Spawn srv;
add_turtle.call(srv);
// 创建发布turtle2速度控制指令的发布者
ros::Publisher turtle_vel = node.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel", 10);
// 创建tf的监听器
tf::TransformListener listener;
ros::Rate rate(10.0);
while (node.ok())
{
// 获取turtle1与turtle2坐标系之间的tf数据
tf::StampedTransform transform;
try
{
listener.waitForTransform("/turtle2", "/turtle1", ros::Time(0), ros::Duration(3.0));
listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform);
}
catch (tf::TransformException &ex)
{
ROS_ERROR("%s",ex.what());
ros::Duration(1.0).sleep();
continue;
}
// 根据turtle1与turtle2坐标系之间的位置关系，发布turtle2的速度控制指令
geometry_msgs::Twist vel_msg;
vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(),
transform.getOrigin().x());
vel_msg.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) +
pow(transform.getOrigin().y(), 2));
turtle_vel.publish(vel_msg);
rate.sleep();
}
return 0;
};
运行结果
同上
6. Matrix3x3
它是一个3x3的矩阵，有3个Vector3 m_el[3]，(此时使用的是行向量)
定义及赋值
//用四元数构造3x3旋转矩阵
Matrix3x3(const Quaternion& q)
Matrix3x3(const tfScalar& xx, const tfScalar& xy, const tfScalar& xz,
const tfScalar& yx, const tfScalar& yy, const tfScalar& yz,
const tfScalar& zx, const tfScalar& zy, const tfScalar& zz)
//赋值构造函数
Matrix3x3 (const Matrix3x3& other)
常用的函数
//四元数转旋转矩阵
void setRotation(const Quaternion& q)
//欧拉角 转旋转矩阵
setRPY(tfScalar roll, tfScalar pitch,tfScalar yaw)
//旋转矩阵转四元数
getRotation(Quaternion& q)
// 旋转矩阵转欧拉角
getRPY(tfScalar& roll, tfScalar& pitch, tfScalar& yaw, unsigned int solution_number = 1)
//矩阵转置
Matrix3x3 transpose() const;
//矩阵求逆
Matrix3x3 inverse() const;
还有如下函数操作
运算符重载=，*=
测试代码
inline void printQuaternion(const tf::Quaternion& q, const std::string& info)
{
// std::cout << "quaternion: " << q[0] << ", " << q[1] << ", " << q[2] << ", " << q[3] << std::endl; // x,y,z,w
std::cout << info << " quaternion: " << q.x() << ", " << q.y() << ", " << q.z() << ", " << q.w() << std::endl; // 两种元素访问方式均可
}
inline void printMatrix(const tf::Matrix3x3& mat, const std::string info)
{
std::cout << info << "-------------------- " << std::endl;
std::cout << std::setprecision(4) << mat[0][0] << "\t" << mat[0][1] << "\t" << mat[0][2] << "\n"
<< mat[1][0] << "\t" << mat[1][1] << "\t" << mat[1][2] << "\n"
<< mat[2][0] << "\t" << mat[2][1] << "\t" << mat[2][2] << "\n\n";
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "transform_ros");
ros::NodeHandle nh;
// 1, 旋转矩阵初始化方法：
double theta = M_PI / 3.0;
double ct = cos(theta), st = sin(theta);
double roll = M_PI / 2.0, pitch = M_PI / 3.0, yaw = M_PI / 4.0;
tf::Quaternion q(0, 0, 0, 1); // x,y,z,w;
tf::Matrix3x3 mat1(q); // 从四元数初始化旋转矩阵, 内部通过 setRotation 实现;
tf::Matrix3x3 mat2(ct, -st, 0, st, ct, 0, 0, 0, 1); // 直接初始化;
tf::Matrix3x3 mat3(mat2);
tf::Matrix3x3 mat4 = mat3;
tf::Matrix3x3 mat5;
mat5.setRPY(roll, pitch, yaw); // 欧拉角到旋转矩阵的变换, 内部直接调用 setEulerYPR(yaw, pitch, roll) 函数来实现;
printMatrix(mat1, "mat1:");
printMatrix(mat2, "mat2:");
printMatrix(mat3, "mat3:");
printMatrix(mat4, "mat4:");
printMatrix(mat5, "mat5:");
// 2, 旋转矩阵与其它旋转量间的转换:
// rotation matrix --> euler:
mat2.getRPY(roll, pitch, yaw, 1); // 第四个参数 solution_number, 缺省时默认值为1；
// 函数内部是直接使用 getEulerYPR(yaw, pitch, roll, solution_number); 方法实现的;
std::cout << "rpy from mat2: " << roll << ", " << pitch << ", " << yaw << std::endl;
mat5.getEulerYPR(yaw, pitch, roll);
std::cout << "rpy from mat5(solution_number not set): " << roll << ", " << pitch << ", " << yaw << std::endl;
mat5.getEulerYPR(yaw, pitch, roll, 1);
std::cout << "rpy from mat5(solution_number = 1): " << roll << ", " << pitch << ", " << yaw << std::endl;
mat5.getEulerYPR(yaw, pitch, roll, 0);
std::cout << "rpy from mat5(solution_number = 0): " << roll << ", " << pitch << ", " << yaw << std::endl;
// 可以看到，solution_number 为0和1时得到的结果时不一样的，它设置为默认值1时的结果与原结果是一致的，所以强烈建议使用getRPY函数时，直接缺省第四个参数；
// 其中的差别主要在于solution_number = 0时，对计算得到的pitch有：pitch = M_PI - pitch; 然后利用该pitch计算roll和yaw，使得最后的结果不一样；
// rotation matrix --> quaternion:
mat2.getRotation(q);
printQuaternion(q, "quaternion from matrix mat2: ");
// quaternion --> rotation matrix:
mat1.setRotation(q);
printMatrix(mat1, "\nmat from q(from mat2): ");
// rotation matrix --> euler:
// mat.getRPY(roll, pitch, yaw);
// euler --> rotation matrix:
// mat.setRPY(roll, pitch, yaw);
// 3, 其它操作:
tf::Vector3 vec1, vec2, vec3;
vec1 = mat1[2];
vec2 = mat1.getColumn(1);
vec3 = mat2.getRow(0);
std::cout << "get column of matrix[2]: " << vec1[0] << ", " << vec1[1] << ", " << vec1[2] << std::endl;
std::cout << "get column of matrix[1]: " << vec2[0] << ", " << vec2[1] << ", " << vec2[2] << std::endl;
std::cout << "get row of matrix[0]: " << vec3[0] << ", " << vec3[1] << ", " << vec3[2] << std::endl;
return 0;
}
运行结果
7. MessageFilter
tf下的消息过滤器。
tf::MessageFilter可以订阅任何的ROS消息，然后将其缓存，直到这些消息可以转换到目标坐标系，然后进行相应的处理（一般在回调函数中处理）。说白了就是消息订阅+坐标转换。实际上，后者继承于前者：
- 定义数据：TransformListener、message_filters::Subscriber、tf::MessageFilter
- 用消息的名称来初始化message_filters::Subscriber
- 用tf、message_filters::Subscriber、目标坐标系来初始化tf::MessageFilter
- 给tf::MessageFilter注册callback。
- 编写callback，并在回调中完成坐标转换。至此完成消息订阅+坐标转换
测试代码
message_filters::Subscriber<std_msgs::UInt32> sub(nh, "my_topic", 1);
sub.registerCallback(myCallback);
8. TimeCache
这个类构建并维护一个带有时间戳的数据列表。并提供查找函数，以根据时间的变化获取数据。
9. TransformException
TF异常
所有在TF里异常继承自tf::TransformException，它继承自std::runtime_error。
异常类型
tf::ConnectivityException
如果由于两个坐标系ID不在同一个连接的树中而无法完成请求，则抛出。
tf::ExtrapolationException
如果请求的坐标系id之间存在连接，但一个或多个变换已过期，则抛出。
tf::InvalidArgument
如果参数无效则抛出。 最常见的情况是非规范化的四元数。
tf::LookupException
如果引用了未发布的坐标系ID，则抛出。
测试代码
catch (tf::TransformException &ex)
{
ROS_ERROR("%s",ex.what());
ros::Duration(1.0).sleep();
continue;
}
扫一扫
2982
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
