ROS学习写一个TF广播

怎么去广播坐标转换？

代码

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <turtlesim/Pose.h>

std::string turtle_name;



void poseCallback(const turtlesim::PoseConstPtr& msg){
  static tf::TransformBroadcaster br;
  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);
  br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
}

int main(int argc, char** argv){
  ros::init(argc, argv, "my_tf_broadcaster");
  if (argc != 2){ROS_ERROR("need turtle name as argument"); return -1;};
  turtle_name = argv[1];

  ros::NodeHandle node;
  ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);

  ros::spin();
  return 0;
};

代码解释

#include <tf/transform_broadcaster.h>

tf包提供了TransformBroadcaster的实现，以帮助简化发布转换的任务。 要使用TransformBroadcaster，我们需要包含tf / transform_broadcaster.h头文件。

 static tf::TransformBroadcaster br;

在这里，我们创建一个TransformBroadcaster对象，稍后我们将使用该对象通过线路发送转换。

  tf::Transform transform;
  transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );
  tf::Quaternion q;
  q.setRPY(0, 0, msg->theta);

在这里，我们创建一个Transform对象，并将2D龟姿势中的信息复制到3D变换中

transform.setRotation(q);

这里设置旋转

 br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));

这是真正的工作所在。 使用TransformBroadcaster发送变换需要四个参数。

• 首先，我们传递变换本身。

• 现在我们需要给变换发布一个时间戳，我们只用当前时间标记它，ros :: Time :: now（）。

• 然后，我们需要传递我们正在创建的链接的父框架的名称，在本例中为“world”

• 最后，我们需要传递我们正在创建的链接的子框架的名称，在这种情况下，这是乌龟本身的名称。

注意：sendTransform和StampedTransform具有父和子的相反顺序。