ROS学习笔记:tf的学习和使用(二)

1.增加一个坐标系

前面我们的效果是让turtle2跟着turtle1走,那么做到有一定布局地跟随呢?事实上原理比较简单,我们只需再构造一个turtle1的子坐标系,然后让turtle2跟着这个子坐标系走即可。

需要注意的是,每个子坐标系都只能有一个父坐标系,而一个坐标系是可以有多个子坐标系的。

 $ roscd learning_tf
src/frame_tf_broadcaster.cpp

   1 #include /ros.h>
   2 #include /transform_broadcaster.h>
   3 
   4 int main(int argc, char** argv){
   5   ros::init(argc, argv, "my_tf_broadcaster");
   6   ros::NodeHandle node;
   7 
   8   tf::TransformBroadcaster br;
   9   tf::Transform transform;
  10 
  11   ros::Rate rate(10.0);
  12   while (node.ok()){
  13     transform.setOrigin( tf::Vector3(0.0, 2.0, 0.0) );
  14     transform.setRotation( tf::Quaternion(0, 0, 0, 1) );
  15     br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "turtle1", "carrot1"));
  16     rate.sleep();
  17   }
  18   return 0;
  19 };
核心代码解释

  13     transform.setOrigin( tf::Vector3(0.0, 2.0, 0.0) );
  14     transform.setRotation( tf::Quaternion(0, 0, 0, 1) );
  15     br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "turtle1", "carrot1"));
setOrigin是设置这个子坐标系与父坐标系的初始位置关系,三个参数分别问x,y,z,z一般设置为0.

setRotation是设置这个子坐标系与父坐标系的角度关系,前三个参数为pitch,row,yaw,最后一个参数为角速度。

最后一行就是继承的父坐标系和子坐标系名称和时间起点。


修改src/turtle_tf_listener.cpp

   1   listener.lookupTransform("/turtle2", "/carrot1",
   2                            ros::Time(0), transform);

修改CMakeLists.txt

add_executable(frame_tf_broadcaster src/frame_tf_broadcaster.cpp)
target_link_libraries(frame_tf_broadcaster ${catkin_LIBRARIES})
在launch文件中添加

              name="broadcaster_frame" />
编译运行

 $ catkin_make
 $ roslaunch learning_tf start_demo.launch
事实上,我们对参数进行一些简单的修改就能看到一些有趣的效果

   1     transform.setOrigin( tf::Vector3(2.0*sin(ros::Time::now().toSec()), 2.0*cos(ros::Time::now().toSec()), 0.0) );
   2     transform.setRotation( tf::Quaternion(0, 0, 0, 1) );
例如改变初始位置为一个时间的正余弦函数,那么很自然的,turtle2便会绕turtle1做一个圆周运动。


2.tf和时间漫游

在之前的例子里,我们的lookupTransform()函数的时间参数都是ros::Time(0),这里的0就意味着是最近时刻采样的坐标系。这个参数是可以变化的,比如我们将其改为ros::Time::now(),now的意思就是当前的坐标系。但是这时候如果直接运行就睡出现如下错误


这是因为一个坐标系从一个节点广播到被另一个节点接收,是需要一定时间的,所以直接订阅当前时间节点的坐标系是找不到的,我们可以做如下修改,等待一个时间确保广播的数据已经被收到。

  try{
    ros::Time now = ros::Time::now();
    listener.waitForTransform("/turtle2", "/turtle1",
                              now, ros::Duration(3.0));
    listener.lookupTransform("/turtle2", "/turtle1",
                             now, transform);
之后再运行就会如我们预期一样。

我们可以再做一些有趣的尝试,比如turtle2不再到达turtle1目前的位置,而是到达turtle2 5秒前所在的位置。

  try{
    ros::Time past = ros::Time::now() - ros::Duration(5.0);
    listener.waitForTransform("/turtle2", "/turtle1",
                              past, ros::Duration(1.0));
    listener.lookupTransform("/turtle2", "/turtle1",
                             past, transform);
但是结果貌似和我们的预期有很大差距

ROS学习笔记:tf的学习和使用(二)_第1张图片

开始的诡异轨迹可以理解为我们并没有turtle1在0时刻以前的数据,那之后呢?所以,显然这里面有一个隐藏的错误。

我们回过头来看上面那段代码,你会发现,我们输入量是5秒之前的turtle1,但是,我们忽略了我们控制的也是5秒之前的turtle2。

很显然,我们希望达到的目的是利用5秒之前的turtle1的信息来控制当前的turtle2的位置,因此我们需要对代码做一些修改

  try{
    ros::Time now = ros::Time::now();
    ros::Time past = now - ros::Duration(5.0);
    listener.waitForTransform("/turtle2", now,
                              "/turtle1", past,
                              "/world", ros::Duration(1.0));
    listener.lookupTransform("/turtle2", now,
                             "/turtle1", past,
                             "/world", transform);
这样结果就和我们预期的一样了。

ROS学习笔记:tf的学习和使用(二)_第2张图片








你可能感兴趣的:(ROS)