一.tf:
一.transformBroadcaster(类):
1.
<code>
void TransformBroadcaster::sendTransform(const StampedTransform & transform)
{
geometry_msgs::TransformStamped msgtf;
transformStampedTFToMsg(transform, msgtf);
tf2_broadcaster_.sendTransform(msgtf);
}
</code>
二.
1.
<code>
StampedTransform(const tf::Transform& input, const ros::Time& timestamp,
const std::string & frame_id, const std::string & child_frame_id)
</code>
三.Transform:
1.
<code>
TFSIMD_FORCE_INLINE void setOrigin(const Vector3& origin)
{
m_origin = origin;
}
</code>
<code>
TFSIMD_FORCE_INLINE void setRotation(const Quaternion& q)
{
m_basis.setRotation(q);
}
void setRotation(const Quaternion& q)
{
tf2Scalar d = q.length2();
tf2FullAssert(d != tf2Scalar(0.0));
tf2Scalar s = tf2Scalar(2.0) / d;
tf2Scalar xs = q.x() * s, ys = q.y() * s, zs = q.z() * s;
tf2Scalar wx = q.w() * xs, wy = q.w() * ys, wz = q.w() * zs;
tf2Scalar xx = q.x() * xs, xy = q.x() * ys, xz = q.x() * zs;
tf2Scalar yy = q.y() * ys, yz = q.y() * zs, zz = q.z() * zs;
setValue(tf2Scalar(1.0) - (yy + zz), xy - wz, xz + wy,
xy + wz, tf2Scalar(1.0) - (xx + zz), yz - wx,
xz - wy, yz + wx, tf2Scalar(1.0) - (xx + yy));
}
</code>
四Quaternion:
<code>
void setRPY(const tfScalar& roll, const tfScalar& pitch, const tfScalar& yaw)
{
tfScalar halfYaw = tfScalar(yaw) * tfScalar(0.5);
tfScalar halfPitch = tfScalar(pitch) * tfScalar(0.5);
tfScalar halfRoll = tfScalar(roll) * tfScalar(0.5);
tfScalar cosYaw = tfCos(halfYaw);
tfScalar sinYaw = tfSin(halfYaw);
tfScalar cosPitch = tfCos(halfPitch);
tfScalar sinPitch = tfSin(halfPitch);
tfScalar cosRoll = tfCos(halfRoll);
tfScalar sinRoll = tfSin(halfRoll);
setValue(sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw, //x
cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw, //y
cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw, //z
cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw); //formerly yzx
}
</code>