第一种方法:来源于 http://wiki.ros.org/urdf/Tutorials/Using urdf with robot_state_publisher
cpp源码如下:(相比较于官方的源码,删去了sensor_msgs::JointState部分,只考虑tf的创建与发布)
int main(int argc, char** argv) {
ros::init(argc, argv, "state_publisher");
ros::NodeHandle n;
tf::TransformBroadcaster broadcaster;
const double degree = M_PI/120;
double angle = 0;
// message declarations
/* This expresses a transform from coordinate frame header.frame_id
to the coordinate frame child_frame_id*/
geometry_msgs::TransformStamped odom_trans;
odom_trans.header.frame_id = "odom";
odom_trans.child_frame_id = "box";
ros::Rate loop_rate(30);
while (ros::ok()) {
// update transform
// (moving in a circle with radius=2)
odom_trans.header.stamp = ros::Time::now();
odom_trans.transform.translation.x = cos(angle)*2; //公转半径
odom_trans.transform.translation.y = sin(angle)*2; //公转半径
odom_trans.transform.translation.z = 0;
odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(angle+M_PI/2);
//send the joint state and transform
broadcaster.sendTransform(odom_trans);