Fork me on GitHub

学习ros-turtle-TF坐标转换与roslaunch的编写

字数统计: 2.3k字   |   阅读时长≈ 9分

在机器人系统中，我们经常会遇到坐标变换相关的问题。比如说，在一个导航系统中，我们通过激光雷达、双目摄像头等传感器对周围环境进行感知，但是这些传感器直接采集的数据是相对于传感器而言的， 而传感器在机器人身上的安装位置又各不相同，我们关心的则是外部环境与机器人本体之间的关系，所以还需要把相对于传感器的数据变换成为相对于机器人的数据。这个过程就是所谓的坐标变换。

如果涉及到多个机器人之间的协同工作，则需要维护两两之间的相对位置关系，在机器人很多的情况下，或者说是自由度很多的情况下，这种坐标系之间的变换关系就相对比较复杂。 ROS系统提供了一个叫做tf的坐标变换库，这个库使得我们能够在各种坐标系之间自由地切换。

在ROS中有一个turtle的例子，一个通过键盘控制turtle1跑，另外一个turtle2跟在turtle1后面，跟随其坐标进行跟踪。

这是：官方例子

这一过程就是通过tf来实现的。一个tf系统由tf广播者(tf broadcaster)和tf监听者(tf listener)两个部分构成的。这里的白龟(turtle1)和红龟(turtle2)就是两个tf广播者，它们不断的向外发送自己相对于世界坐标系的位置(也就是自己坐标系的原点在世界坐标系下的位置)。这里的是广播，所以所有的都会接收到他们发送的。
另外有一个监听者负责维护两个乌龟的坐标系，通过tf的API系统计算白龟(turtle1)相对红龟(turtle2)的位置，并控制红龟(turtle2)跟踪白龟(turtle1)。为了跟随，就需要知道turtle1相对于turtle2的坐标位置，也就是说turtle1的坐标原点在turtle2下的位置，也就是说turtle1在turtle2下的位置

官方broadcaster例子C++版

创建功能包及其依赖
catkin_create_pkg learning_tf tf roscpp rospy turtlesim

broadcaster.cpp，我们发布的节点就是通过这个来广播自己在世界坐标系的位置

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
//这里有我们访问master的接口
#include <ros/ros.h>     

//提供了tf广播的各种API 后面的listen就是根据这个来的
#include <tf/transform_broadcaster.h>   

//turtlesim是一个依赖包，里面有将要订阅pose主题的消息类型
#include <turtlesim/Pose.h>

std::string turtle_name;


//这里的const turtlesim::PoseConstPtr& msg 是一个套路，目前还不知道这个
//是啥意思？
void poseCallback(const turtlesim::PoseConstPtr& msg){
  //tf广播器，发布话题相当于定义一个发布器
  static tf::TransformBroadcaster br;
  
  /**根据乌龟当前的位姿，设置相对于世界坐标系的坐标变换
  ** 位姿就是海龟仿真器发出的，也就是/sim 这个sim就是海龟仿真器 <node pkg="turtlesim" type="turtlesim_node" name="sim"/>
  **这里启动时的名字
  **/
  
  //定义存放转换信息(平动 转动)的变量
  tf::Transform transform;
  
  /**设置坐标原点
  **setOrigin()函数的参数类型需要为tf::Vector3类型
  **(msg->x, msg->y, 0.0) 为turtle的坐标原点在world世界下的坐标/
  transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );
  
  //定义旋转
  tf::Quaternion q;
  
  //setRPY函数的参数为turtle在world坐标系下的roll,pitch,yaw
  q.setRPY(0, 0, msg->theta);
  transform.setRotation(q);
  
  /**br有成员函数send**  这是 Transform自带的
  ** 第一个参数是坐标变换内容，第二个参数是时间戳
  ** world 和 turtle_name 表示本次坐标变换相对于世界坐标系的变换
  **这里world就是父坐标系 turtle_name就是子坐标系
  **同时，这里子坐标系有两个实际上 turtle1 turtle2所以这里取了个变量
  **父坐标系只有一个，就是world
  */
  br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
}

int main(int argc, char** argv){
    //初始化一个节点，名字叫做my_tf_broadcaster
  ros::init(argc, argv, "my_tf_broadcaster");
  /**输入参数有两个类似于这个:rosrun turtlesim turtle1  这个经过测试不行
  **但是，rosrun turtlesim turtlsim_node 却是可以，不知道这里为什么
  **所以，这里输入参数是2个，第二个参数就是turtle_name/
  if (argc != 2){ROS_ERROR("need turtle name as argument"); return -1;};
  turtle_name = argv[1];

  ros::NodeHandle node;
  /**这里turtle_name 就是：turtle1, turtle2
  **这里&poseCallback 前面可以不用加&/
  ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);

   //循环等待
  ros::spin();
  return 0;
};

官方listener例子C++版

turtle_tf_listener.cpp

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
#include <ros/ros.h>
//这里跟前面的不一样，前面是tf/transform_broadcaster.h
#include <tf/transform_listener.h>  
#include <geometry_msgs/Twist.h>   //坐标变换好像要用到这个
#include <turtlesim/Spawn.h>

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

  /**
  **这段就是在图中生成一个turtle,这里猜测，启动的时候已经启动了一个turtle1
  **但是，还需要一个turtle2,所以这里生成一个
  **但是，经过测试发现，这里的话，如果在后面设置的roslaunch 里面没有写的话
  **那么就是只是生成一个turtle，而这个节点是没有启动的，那就更加不用说控制了
  **同理，如果只是在roslaunch里面写了节点，这里不写的话，那么rosnode 里面会有
  **但是，图中不会显示乌龟
  */
  ros::service::waitForService("spawn");
  ros::ServiceClient add_turtle =
    node.serviceClient<turtlesim::Spawn>("spawn");
  turtlesim::Spawn srv;
  add_turtle.call(srv);

    //前面是生成一个turtle2，然后我们要发布cmd_vel速度控制指令来控制
  ros::Publisher turtle_vel =
    node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10);

  //在程序中具现化监听器对象。只要创建了这个对象，它就开始监听系统中的各个坐标变换，并将之缓存10秒。这里也就是存放变换关系的变量
  tf::TransformListener listener;

  ros::Rate rate(10.0);
  while (node.ok()){
    tf::StampedTransform transform;
    try{
       /**通过Transform的API 查询turtle1相对于turtle2的坐标变换
        **跟前面一样，就是turtle1的原点在turtle2下的位置 
        **第三个参数'ros::Time(0)'则指获取当前时刻的坐标关系。 最后一个参数**tranform则记录了具体的变换内容，它是一个tf::StampedTransform对象。
        **且这里tf会把监听的内容放到一个缓存中，然后再读取相关的内容。而这个过程会有几毫秒的误差.所以这里不能像先前的那样，使用 ros::Time::now()
        */
      listener.lookupTransform("/turtle2", "/turtle1",
                               ros::Time(0), transform);
    }
    catch (tf::TransformException &ex) {
      ROS_ERROR("%s",ex.what());
      ros::Duration(1.0).sleep();
      continue;
    }
     /**监听到这两个的坐标变换后，得到turtle2关于turtle1的坐标变换后
      **就可以计算turtle2的速度了，并发布速度指令控制其运动
      **下面的这个还不知道是怎么计算的
      */
    geometry_msgs::Twist vel_msg;
    vel_msg.angular.z = 4.0 * 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;
};

最后，在learning_tf下新建launch文件turtle_tf_demo.launch

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
  <launch>
    <!-- 启动海龟仿真，光这一个就可以生成一个海龟turtle1-->
    <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"/>

<!-- 启动两个turtle的广播-->
<!-- pkg是报名,type是boardcaster.cpp编译CMakeLists里面编译的可执行文件的名字-->
<!-- args就是传递的参数-->
<!-- name就是运行时节点的名字，本来一开始ros::init里面有名字，这里会把那个覆盖-->
<!-- 但是，这里name也是必须的，不能删除，同时,ros::init初始化那里也不能删除
感觉就是一个占位的，反正这里也会把它进行覆盖-->
    <node pkg="learning_tf" type="turtle_tf_broadcaster"
          args="/turtle1" name="turtle1_tf_broadcaster" />
    <node pkg="learning_tf" type="turtle_tf_broadcaster"
          args="/turtle2" name="turtle2_tf_broadcaster" />
    
    <!--启动监听节点，观察发现，这里就没有args参数，是因为这里不需要进行参数的传递前面是需要得到turtle在世界坐标系的位置并发布出去需要传递turtle的名字进去-->
    <node pkg="learning_tf" type="turtle_tf_listener"
          name="listener" />
  </launch>

最后在CMakeLists.txt中添加

1
2
3
4
5
add_executable(turtle_tf_broadcaster src/turtle_tf_broadcaster.cpp)
target_link_libraries(turtle_tf_broadcaster ${catkin_LIBRARIES})

add_executable(turtle_tf_listener src/turtle_tf_listener.cpp)
target_link_libraries(turtle_tf_listener ${catkin_LIBRARIES})
赏

谢谢你请我吃糖果

支付宝
微信

扫一扫，分享到微信

tag:

    缺失模块。
    1、请确保node版本大于6.2
    2、在博客根目录（注意不是yilia-plus根目录）执行以下命令：
    npm i hexo-generator-json-content --save

    3、在根目录_config.yml里添加配置： 

      jsonContent:
    meta: false
    pages: false
    posts:
      title: true
      date: true
      path: true
      text: false
      raw: false
      content: false
      slug: false
      updated: false
      comments: false
      link: false
      permalink: false
      excerpt: false
      categories: false
      tags: true

学生，现居成都
学习无人机px4目标识别与跟踪