坐标转换tf

在src文件下新建一个.cpp写入以下代码

// 头文件包含
#include 
#include 
#include 
#include 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;add_turtle.call(srv);// 创建一个话题发布器，发布第二只乌龟运行的速度ros::Publisher turtle_vel =node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10);// 创建tf侦听器listenertf::TransformListener listener;ros::Rate rate(10.0);while (node.ok()){tf::StampedTransform transform;try{// 最多等待10秒，如果提前得到了坐标的转换信息，直接结束等待listener.waitForTransform("/turtle2", "/turtle1", ros::Time(0), ros::Duration(10.0));// 从tf上侦听turtle2在turtle1坐标系中的位置,将该位置存储在transform中listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0),  transform);}catch (tf::TransformException &ex) {ROS_ERROR("%s",ex.what());ros::Duration(1.0).sleep();continue;}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));// 计算turtle2跑的速度和方向turtle_vel.publish(vel_msg);rate.sleep();}return 0;
};

修改.txt文件

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

编译： 在.中添加以下代码：

运行

.

效果

一般一步实现了前面例子的乌龟跟随

运行

分析是跟着跑还是跟着跑？

添加一个坐标系 为什么要添加坐标系？ 操作小记

在src//src下创建文件.cpp，写入以下代码：

#include 
#include 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) );// 发布一个以turtle1为父级命名为carrot1的坐标系br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "turtle1", "carrot1"));rate.sleep();}return 0;
};

在.txt中添加如下代码：

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

编译：

在.中添加如下代码：

运行 .

运行命令查看tf树：

此时，操作键盘控制乌龟移动，发现跟之前的行为并没有什么区别

在.cpp文件中做以下修改:

try{listener.waitForTransform("/turtle2", "/carrot1", ros::Time(0), ros::Duration(10.0));listener.lookupTransform("/turtle2", "/carrot1", ros::Time(0),  transform);
}

重新编译运行，查看效果跟之前的区别

之前，我们创建了一个固定关系的坐标系，它相对于父级是不会变化的。现在创建一个动态的坐标系

修改.cpp中对应的代码:

  while (node.ok()){transform.setOrigin( tf::Vector3(2.0*sin(ros::Time::now().toSec()), 2.0*cos(ros::Time::now().toSec()), 0.0) );transform.setRotation( tf::Quaternion(0, 0, 0, 1) );br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "turtle1", "carrot1"));rate.sleep();}

编译运行，观察现象

tf和时间 操作小记

打开.cpp，原来代码为:

ros::Time(0)参数指的就是获取当前最新的转化关系，将代码做如下修改:

该段代码是指获取当前时间点的转换关系

编译：

运行： .

运行时会出现一个错误：该错误是由于的生成需要一定的时间，当我们第一次要询问转换关系时，还没有生成

如何获取5S前的坐标系转换关系？

修改代码如下:

编译：

运行： .

感觉第二只小乌龟完全不受控！

原因在于，我们获取的相对关系是5s前的，5s前的和之间的关系

如何让跟着 5s前的位置运行呢？

一共包含了6个参数：

要转换的目标坐标系；在什么时间；源坐标系；在什么时间；不随时间变化的坐标系；存储结果的变量

上图显示了坐标系转换的过程：

tf计算在past的时间点和世界坐标系之间的关系随着时间推移到达现在的时间点，计算和世界坐标系之间的关系tf计算当前和past时间点之间的关系

编译：

运行： .