如何设置Python中turtle角色的初始坐标

一．ＴＦ简介

ＴＦ简介：机器人系统通常具有许多随时间变化的坐标系，例如世界坐标系、基座坐标系、夹持器坐标系、头部坐标系等。TF是一个变换系统，它允许在一个坐标系中进行计算，然后在任何所需的时间点将它们转换到另一个坐标系。TF能够回答以下问题：地图坐标系中机器人基座的当前位姿是什么？相对于机器人基座，夹持器中物体的位姿是什么？5秒前，头部坐标系在世界坐标系的什么地方？(注意：这段话里面重要的有两点，一个是不同坐标系的转换，另一个是时间的转换)

TF的优点：分布式系统，即没有单点故障；转换多次时没有数据丢失；没有坐标系之间的中间数据转换的计算成本；用户无需担心其数据的起始坐标系；有关过去位置的信息进行存储并且可访问（在本地记录开始后）。

ＴＦ节点：主要是两个一个是发布器，作用是发布坐标系之间的转换关系，主要是tf::Transform 到话题/tf；另一个是订阅器listener，直接订阅/tf并缓存所有的数据到缓存极限．

TF变换树。TF在坐标系之间构建变换树，支持多个非连接树，仅在同一树中变换有效，见下图

不同节点就是不同坐标系之间的变换关系就如上图所示，相邻两个节点间的变换关系只有一条，这个很重要，是保证迭代转换的前提．

有些复杂的工程系统可能有几十个坐标系需要转换，tf可以帮助我们完成这个功能，让我们的主要精力放在算法的设计上．

二．如何使用ＴＦ树

假设我们希望robot2根据robot1的激光数据进行导航，给定TF树，

要用到/robot1/laser-------->/robot2/odom:

整个过程用到了Inverse Transform 和Forward Transform两个转换

可以跑一个demo来演示一下，

roslaunch turtle_tf turtle_tf_demo.launch

 我显示了这个：

不知道是什么错误，不是重点，暂时不管．

发现是没有安装包turtle_tf，安装一下就好了：

sudo apt-get install ros-melodic-turtle-tf

 显示控制一个海龟，然后另一个海龟跟随．

分析具体的launch文件的内容

<launch>

 <!-- Turtlesim Node-->
 <node pkg="turtlesim" type="turtlesim_node" name="sim"/>

 <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>
 <!-- Axes -->
 <param name="scale_linear" value="2" type="double"/>
 <param name="scale_angular" value="2" type="double"/>

 <node name="turtle1_tf_broadcaster" pkg="turtle_tf" type="turtle_tf_broadcaster.py" respawn="false" output="screen" >
   <param name="turtle" type="string" value="turtle1" />
 </node>
 <node name="turtle2_tf_broadcaster" pkg="turtle_tf" type="turtle_tf_broadcaster.py" respawn="false" output="screen" >
   <param name="turtle" type="string" value="turtle2" />
 </node>
 <node name="turtle_pointer" pkg="turtle_tf" type="turtle_tf_listener.py" respawn="false" output="screen" >
 </node>

</launch>

该Demo使用TF库创建3个坐标系，分别是世界坐标系、turtle1坐标系和turtle2坐标系；包含一个TF广播器（broadcaster）发布turtle坐标系，一个TF订阅器（listener）计算两个turtle坐标系之间的距离。

不是重点，略过．

三．ＴＦ相关工具

view_frames：可视化完整的坐标变换树；

通过view_frames创建一个由TF使用ROS广播的坐标系变换树

再用下面命令查看

evince frames.pdf

tf_monitor：监视坐标系之间的变换； 

rosrun tf tf_moniter

tf_echo：将指定的变换输出到屏幕；

tf_echo通过ROS广播任意两个坐标系之间的变换，使用示例：

$ rosrun tf tf_echo [reference_frame] [target_frame]

让我们看一下turtle2坐标系相对于turtle1坐标系的变换，它相当于：

就是说turtle1---->turtle2 等价于turtle1--->world--->turtle2

具体命令表示：

rosrun tf tf_echo turtle1 turtle2

roswtf：使用tfwtf插件，帮助我们跟踪TF的问题；

static_transform_publisher：一个用于发送静态变换的命令行工具。

四．广播ＴＦ变换

TF广播器将坐标系的相对位姿发送给系统的其余部分，一个系统可以有多个广播器，每个广播器提供机器人不同部分的信息。接下来，我们将编写代码来重现上述tf Demo。

首先新建一个名为lecture1-6的新包，依赖包有tf,roscpp,rospy,turtlesim

然后在src文件下新建一个tf_broadcaster.cpp的文件，内容如下：

#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;
   // 设置T矩阵
   transform.setOrigin(tf::Vector3(msg->x, msg->y, 0.0));
   tf::Quaternion quaternion;
   transform.setRotation(tf::createQuaternionFromYaw(msg->theta));

   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 nh;
   ros::Subscriber sub = nh.subscribe(turtle_name+"/pose", 10, &poseCallback);

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

比较简单，相关函数在上一篇笔记有所讲解．

然后在launch中，新建tf_demo.launch文件：

<launch>
    <!-- Turtlesim Node-->
    <node pkg="turtlesim" type="turtlesim_node" name="sim"/>
    <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>

    <!-- tf broadcaster node -->
    <node pkg="lecture1-6" type="tf_broadcaster"
          args="/turtle1" name="turtle1_tf_broadcaster" />
  </launch>

关键是args里面是我们在运行是需要输入的海龟的名称

运行结果：

 运用前面的命令查询是否成功订阅到turtle的tf

rosrun tf tf_echo /world /turtle1

这个命令是显示/world--->/turtle1的变换

突发奇想，如果颠倒位置会怎么样？

rosrun tf tf_echo /turtle1 /world

应该显示/turtle1--->/world的变换

 正好相反，总是在学新东西的时候有些奇奇怪怪的想法．．．

之后就是接受模块，在src下创建tf_listener.cpp:

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <turtlesim/Spawn.h>
#include <geometry_msgs/Twist.h>

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

    ros::NodeHandle node;
    // 参数服务器，生成一只乌龟
    ros::service::waitForService("spawn");
    ros::ServiceClient add_turtle =
       node.serviceClient<turtlesim::Spawn>("spawn");
    turtlesim::Spawn srv;
    add_turtle.call(srv);

    ros::Publisher turtle_vel =
       node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10);

    tf::TransformListener listener;
    ros::Rate rate(10.0);

    while (node.ok()){
        tf::StampedTransform transform;
        try {
            listener.waitForTransform("/turtle2", "/turtle1", ros::Time(0), ros::Duration(10.0) );
            listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform);
        } catch (tf::TransformException &ex) {
            ROS_ERROR("%s",ex.what());
        }

        geometry_msgs::Twist vel_msg;
        vel_msg.angular.z = 4 * 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;
};

核心函数是listener.waitForTransform()和listener.lookupTransform()

之前碰到这两个函数总是搞不清楚，到底是第一个参数到第二个参数的转换，还是反过来，现在在这里把它彻底弄清楚．

关于listener.waitForTransform()

/** \brief Block until a transform is possible or it times out
   * \param target_frame The frame into which to transform
   * \param source_frame The frame from which to transform
   * \param time The time at which to transform
   * \param timeout How long to block before failing
   * \param polling_sleep_duration How often to retest if failed
   * \param error_msg A pointer to a string which will be filled with why the transform failed, if not NULL
   */
  bool waitForTransform(const std::string& target_frame, const std::string& source_frame,
                        const ros::Time& time, const ros::Duration& timeout, const ros::Duration& polling_sleep_duration = ros::Duration(0.01),
                        std::string* error_msg = NULL) const;

这里的/param target_frame The frame into which to transform的意思就是要转换的目标frame，那这里就是说turtle1--->turtle2

关于listener.lookupTransform()

/** \brief Get the transform between two frames by frame ID.
   * \param target_frame The frame to which data should be transformed
   * \param source_frame The frame where the data originated
   * \param time The time at which the value of the transform is desired. (0 will get the latest)
   * \param transform The transform reference to fill.  
   *
   * Possible exceptions tf::LookupException, tf::ConnectivityException,
   * tf::MaxDepthException, tf::ExtrapolationException
   */
  void lookupTransform(const std::string& target_frame, const std::string& source_frame,
                       const ros::Time& time, StampedTransform& transform) const;

按源码的解释应该也是turtle1--->turtle2吧．

之后可以测试一下，使用笔记一中的源码，魔改一下．

基本实现了之前的turtle_tf_demo.launch

五．测试想法

测试之前的想法，我的验证想法思路是这样的，先广播设定好的一个坐标系之间的变换关系，然后在用另一个listener.cpp文件中使用这两个函数进行输出．

首先创建一个test_tf功能包，依赖有tf, roscpp, rospy

首先在src中创建test_tf_broadcaster.cpp,内容如下：
 

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

int main(int argc, char** argv)
{
    ros::init(argc, argv, "publishe_tf");
    ros::NodeHandle nh;
    ros::Rate r(100);

    // 在我的假设中oxis1 ---> oxis2 是(-1, -2, -3)
    //                            oxis2 ---> oxis1 是(1, 2, 3)
    // 所以按我的理解下面的函数发送的就是oxis1 ---> oxis2,其实这里的源码的注释部分已经说的很详细了。
//并且以oxis1为参考坐标系
    tf::TransformBroadcaster br;
    while(nh.ok())
    {
        br.sendTransform(
            tf::StampedTransform(
                tf::Transform(tf::Quaternion(0,0,0,1),tf::Vector3(-1,-2,-3)),
                ros::Time::now(),
                "/oxis1",
                "/oxis2"
            )
        );
        r.sleep();
    }
    return 0;
}

具体的看注释，然后在src下创建test_tf_listener.cpp

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/PointStamped.h>

int main(int argc, char** argv)
{
    ros::init(argc, argv, "listen_tf");
    ros::NodeHandle nh;

    tf::TransformListener listener;
    tf::StampedTransform transform;

    listener.waitForTransform("/oxis1", "/oxis2", ros::Time(0), ros::Duration(10.0) );
    listener.lookupTransform("/oxis1", "/oxis2", ros::Time(0), transform);

    std::cout <<"child_frame_id:" <<std::endl;
    std::cout << transform.child_frame_id_ <<std::endl;

    std::cout <<"frame_id:" <<std::endl;
    std::cout << transform.frame_id_ <<std::endl;

    std::cout <<transform.getOrigin().getX()<<
    transform.getOrigin().getY()<<transform.getOrigin().getZ()<<std::endl;

    return 0;
}

之后的结果发现是与lookupTransform中参数的位置相关，

如果参数一是/oxis1,参数二是/oxis2,那么输出的就是我当时发送的，是从/oxis2---->/oxis1的变换。

如果参数一是/oxis2, 参数而是/oxis1,那么就会自动变换符号。

具体可以自己运行源码分析，不要忘了关于CMakeLists.txt文件的修改。

注意，lookupTransform最好与waitForTransform连用，不然会报错。