turtlesim用teleport做坐标变换，不用spawn和角速度做坐标变换

昨天困扰个问题到今天，希望要让turtle2的坐标永远相对于turtle1的坐标都是向某个地方平移的。本来是根据tf_tutorial来做的，建议读者先照着上面的tutorial做一遍

方案1：a.新建的carrot1（由turtle1平移而来）

             b.新建一个broadcaster发布carrot1相对于turtle1的位置

             c.listener让turtle2向carrot1移动

但由于“/turtle/cmd_vel”消息类型是geometry_msgs::Twist，只能从transform中获取线速度和角速度进行平移。

这里要注意一点，我最开始犯的错误就是只建了三个frame（请忽略左边的point！）

我建了三个坐标系world，turtle1，turtle2，然后用两个broadcaster分别发布了world中turtle1的位置和turtle1中turtle2的位置，在listener中直接listener.lookupTransform("/world","/turtle2",ros::Time(0), transform);

这种直接按照tutorial修改而来的方式，运行结果并不能达到要求，因为前面提到了，控制turtle2是用线速度和角速度的，很明显上面获取到的tranform转换而来线速度和角速度的话，会让turtle2一直在原地旋转

方案2: 不使用消息turtle2/cmd_vel（类型为geometry_msgs::Twist，具体包含就是线速度和角速度），查阅turtlesim文档，发现有service为teleport_absolute，使用这个服务而不是使用线速度和角速度对turtle2进行控制，很遗憾的是关于teleport_absolute的例子很少，上面那个wiki界面本来是有视频教程的由于是youtube所以看不了（当然有能力翻墙的小伙伴还是看看为好，我只改了host只能用用google）

            a.同样的，两个广播发布world中turtle1的位置和turtle1中turtle2的位置

            b.listener中同样lookuptransform找出world中turtle2的坐标转换transform，从其中取出x，y然后用teleport_absolute直接指定turtle2的位置

代码如下

 turtle_tf_broadcaster.cpp    和tutorial里面一模一样，作用就是发布turtle1在world中的位置

 #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);
  ros::Time now = ros::Time::now();
  br.sendTransform(tf::StampedTransform(transform, 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];
 
  //turtle_name = "turtle2";
  ros::NodeHandle node;

  //订阅turtle1/pose消息，然后调用回掉函数发布turtle1相对world的位置关系
  ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);


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

turtle12_tf_broadcaster.cpp    发布turtle2在turtle1坐标系中的位置

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


int main(int argc, char** argv)
{
ros::init(argc, argv, "my_tf_broadcaster");
ros::NodeHandle node;
tf::TransformBroadcaster br;
tf::Transform transform;
     
ros::Rate rate(10.0);
while (node.ok()){
transform.setOrigin( tf::Vector3(0.0, 2.0, 0.0) );
//transform.setRotation( tf::Quaternion(0, 0, 0, 1) );
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "/turtle1", "/turtle2")); //parent,son
rate.sleep();
     }
return 0;
}

最后这个文件是重点，turtle_tf_listener.cpp

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>
#include <geometry_msgs/Twist.h>
#include <turtlesim/Spawn.h>
#include <turtlesim/TeleportAbsolute.h>
 void transformPoint(const tf::TransformListener& listener){
   /*we'll create a point in the base_laser frame that we'd like to transform
    *to the base_link frame*/
   geometry_msgs::PointStamped laser_point;
   laser_point.header.frame_id = "turtle2";
   //we'll just use the most recent transform available for our simple example
   laser_point.header.stamp = ros::Time();
   //just an arbitrary point in space
   laser_point.point.x = 0.0;
   laser_point.point.y = 2.0;
   laser_point.point.z = 0.0;
   try{
     geometry_msgs::PointStamped base_point;
     listener.transformPoint("turtle1", laser_point, base_point);
     ROS_INFO("base_laser: (%.2f, %.2f. %.2f) -----> base_link: (%.2f, %.2f, %.2f) at time %.2f",
         laser_point.point.x, laser_point.point.y, laser_point.point.z,
         base_point.point.x, base_point.point.y, base_point.point.z, base_point.header.stamp.toSec());
   }
   catch(tf::TransformException& ex){
     ROS_ERROR("Received an exception trying to transform a point from \"turtle1\" to \"turtle2\": %s", ex.what());
   }
 }






int main(int argc, char** argv){
  ros::init(argc, argv, "my_tf_listener");


  ros::NodeHandle node;


  ros::service::waitForService("spawn");
  ros::ServiceClient add_turtle =
    node.serviceClient<turtlesim::Spawn>("spawn");
  turtlesim::Spawn srv;

//设定turtle2的初始值，这里设置的其实没用，因为运行起来之后下面的teleport会直接把turtle2平移到之前发布的turtle1和turtle2相对的位置去，这里留着做个备忘
  srv.request.x = 4;
  srv.request.y = 1;
  srv.request.theta = 0;

//利用spawn创建新的turtle
  add_turtle.call(srv);

//弃用下面发布消息的方式，改用teleport_absolute  服务的方式控制turtle2的移动
  ros::service::waitForService("turtle2/teleport_absolute");
  ros::ServiceClient teleport_turtle =
    node.serviceClient<turtlesim::TeleportAbsolute>("turtle2/teleport_absolute");
   turtlesim::TeleportAbsolute telesrv;

 //tutorial中使用的是这种方式
  //ros::Publisher turtle_vel =
  //  node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel",10);


  tf::TransformListener listener;


  static tf::TransformBroadcaster br;
  ros::Rate rate(10.0);
  while (node.ok()){
    tf::StampedTransform transform;
    try{
     //直接查找world中turtle2的位置，最后一个参数是设置超时

     //这里其实还有个问题，之前tutorial里面有个time travel的章节，让turtle2跟随turtle1五秒或者几秒之前的坐标

    //但是实际我试验的情况是，如果我的turtle1停着不动，最终turtle2还是会和turtle1重合，这一点我猜测是/turtle1/pose消息发送的频率问题，它是一直在发送的
     listener.waitForTransform("/world","/turtle2",ros::Time(0),ros::Duration(10.0) );
     listener.lookupTransform("/world","/turtle2",ros::Time(0), transform);
    }
    catch (tf::TransformException &ex) {
      ROS_ERROR("%s",ex.what());
      ros::Duration(1.0).sleep();
      continue;
    }
    telesrv.request.x = transform.getOrigin().x();
    telesrv.request.y = transform.getOrigin().y();
    telesrv.request.theta = 0.0;
    teleport_turtle.call(telesrv);

   }
  return 0;
};

tf相关工具

             1.用rosrun rqt_tf_tree rqt_tf_tree 查看各个frames之间的树状关系  本例如图

            2.用rosrun tf tf_echo /point //turtle1   查找turtle1相对于point的运动变换（注：point是固定在world中的任意一点，就是创建一个broadcaster发布point相对world的位置）

            3.用rosrun rqt_graph rqt_graph查看消息的来龙去脉

           4.用rosrun rviz rviz -d `rospack find learning_tf`/rviz/tule_rviz.rviz  查看坐标在rviz下面的变换   learning_tf记得替换为你直接的包名

              这个工具可能会出现Fixed Frame [map] does not exist找不到map frame的情况，开着rviz的情况下再起一个命令行，运行rosrun tf static_transform_publisher 0 0 0 0 0 0 1 map world 10   。。。world为你的父frame名