沉默杀手HomeArchivesTagsCategories
tf(四) 常用的程序
|ROSROS程序和源码分析|
Word count: 1.3k|Reading time: 5 min

使用程序实现父子坐标系的转换

除了直接用static_transform_publisher外，当然可以在程序中实现两个坐标系的转换，以经典的乌龟追乌龟程序为参考，它的tf树是这样的

现在我们写一个这样的程序：

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
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <tf/tf.h>
#include <tf/transform_listener.h>
#include <turtlesim/Pose.h>

void poseCallback(const turtlesim::PosePtr& msg)
{
  static tf::TransformBroadcaster br;
  tf::Transform transform;
  transform.setOrigin( tf::Vector3(0,0, -0.24) );  //子坐标系相对父坐标系的位移
  //子坐标系相对父坐标系的旋转，可以用欧拉角和四元数两种形式
  tf::Quaternion q;
  q.setRPY(0, 0, 0);
  //tf::Quaternion q(qx,qy,qz,qw);
  transform.setRotation(q);
  br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "turtle1", "robot_base"));   //父坐标系在前，子坐标系在后
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "camera_base_tf");
    ros::NodeHandle node;
    ros::Subscriber sub = node.subscribe("/turtle1/pose", 10, &poseCallback);
    ros::spin();
    return 0;
}

我们订阅乌龟1的位姿话题/turtle1/pose，在回调函数里定义tf广播和变换的参数，最后发送。这里只能用回调函数的形式，因为在main函数里只能发一次，无法建立父子坐标系的关系。tf::broadcaster类的sendTransform函数中，把父子坐标系交换，只会交换tf树中的位置，坐标关系还是原来的，所以必须使用Transform::inverse ()函数．

使用程序获得坐标系在另一坐标系中的坐标

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
#include <ros/ros.h>
#include <tf/tf.h>
#include <tf/transform_listener.h>

int main(int argc, char** argv)
{
  ros::init(argc, argv, "turtle_2_1");
  ros::NodeHandle n;
  tf::TransformListener listener;
  ros::Rate rate(10);
  while(n.ok())
  {
    tf::StampedTransform transform;
    try
    {
      listener.waitForTransform("turtle1","turtle2",ros::Time(0),ros::Duration(3));
      listener.lookupTransform("turtle1","turtle2",ros::Time(0), transform);
    }
    catch(tf::TransformException &ex)
    {
      ROS_ERROR("%s",ex.what());
      ros::Duration(1).sleep();
      continue;
    }
    ROS_INFO("x: %f", transform.getOrigin().x());
    ROS_INFO("y: %f", transform.getOrigin().y());
    ROS_INFO("z: %f\n", transform.getOrigin().z());
    rate.sleep();
  }
  return 0;
}

这个程序获得的是乌龟2在乌龟1坐标系中的坐标

先运行roslaunch turtle_tf turtle_tf_demo.launch，这是ROS自带的一个tf演示程序，然后马上运行上面的程序。我们会看到乌龟1不动，乌龟2向前者移动，如果运行程序太慢，乌龟2就停止了，二者没有相对运动就捕捉不到tf话题上的消息了


同时我们的程序会输出下面内容

1
2
3
4
5
6
7
8
9
10
11
12
[ INFO] [1551530137.681192434]: x: -1.394780
[ INFO] [1551530137.681317271]: y: -3.510608
[ INFO] [1551530137.681352733]: z: 0.000000

[ INFO] [1551530137.781891361]: x: -1.221200
[ INFO] [1551530137.781970030]: y: -3.391552
[ INFO] [1551530137.782007398]: z: 0.000000

[ INFO] [1551530137.982427762]: x: -1.018356
[ INFO] [1551530137.982512261]: y: -3.122349
[ INFO] [1551530137.982559203]: z: 0.000000
......	//忽略

显然从图中能看出乌龟2在乌龟1的坐标系中初始坐标x,y肯定都是负值。随着运动，xy的绝对值越来越小，最终两个乌龟重合，xy都成为0。

程序分析

waitForTransform函数的原型有两个，我们用的这个是:

1
2
3
4
5
6
7
8
9
bool Transformer::waitForTransform
(
	const std::string & 	target_frame,
	const std::string & 	source_frame,
	const ros::Time & 		time,	//The time at which to transform
	const ros::Duration & 	timeout,  //How long to block before failing
	const ros::Duration & 	polling_sleep_duration = ros::Duration(0.01),
	std::string * 			error_msg = NULL 
)const

功能：函数阻塞直到能进行坐标系转换或者时间timeout截止，返回值表示是否成功。函数不会throw，如果给error_msg传变量，它会在函数出错时记录错误信息。

一般用前四个参数就行了，如果开始没有坐标系转换，会报错 “turtle1” passed to lookupTransform argument target_frame does not exist 。如果又有了转换，程序可以正常运行。因为网络问题，如果timeout为0，那会直接报错失败，如果设置太小，也可能捕捉不到转换。使用tf_echo就会发现它要等一点时间才会输出结果，这是一样的道理。

lookupTransform函数的原型也有两个，我们用的是:

1
2
3
4
5
6
7
void Transformer::lookupTransform
(	
	const std::string & 	target_frame,
	const std::string & 	source_frame,
	const ros::Time & 	time,  //某时间的变换，0表示最近的变换
	StampedTransform & 	transform 
)const

功能：获得两坐标系之间的转换，也就是赋值给tf::StampedTransform对象

还有函数canTransform()，这个函数纯粹就是判断用的，也能记录错误信息，但不能指定阻塞时间。

技巧 1

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
std::string tf_error;
// make sure that the transform between base_link and the global frame is available
while (ros::ok()
    && !tf_.waitForTransform(global_frame_, "base_link", ros::Time(), ros::Duration(0.1), ros::Duration(0.01), &tf_error))
{
  ros::spinOnce();
  if (last_error + ros::Duration(5.0) < ros::Time::now())
  {
    ROS_WARN("Timed out waiting for transform from %s to %s to become available before running costmap, tf error: %s",
             robot_base_frame_.c_str(), global_frame_.c_str(), tf_error.c_str());
    last_error = ros::Time::now();
  }
  // The error string will accumulate and errors will typically be the same, so the last
  // will do for the warning above. Reset the string here to avoid accumulation.
  tf_error.clear();
}

其他有用的函数

tf::Transformer类的重要函数

  • std::string Transformer::allFramesAsString() const    A way to see what frames have been cached Useful for debugging.

  • bool Transformer::frameExists (const std::string & frame_id_str) const    Check if a frame exists in the tree.

  • void Transformer::getFrameStrings ( std::vector< std::string > & ids ) const    A way to get a std::vector of available frame ids.

  • getParent

1
2
3
4
bool Transformer::getParent ( const std::string &   frame_id,
    ros::Time   time,
    std::string &   parent 
)   const

Fill the parent of a frame.

  • frame_id The frame id of the frame in question
  • parent The reference to the string to fill the parent Returns true unless “NO_PARENT”
  • transformQuaternion()