ROS.tf3编写一个tf接收器(C++)
在之前的教程中,我们创建了一个tf广播公司向tf发布一只乌龟的姿势。 在本教程中,我们将创建一个tf接收器来开始使用tf。 1.1如何创建一个tf接收器 我们先创建源文件。 转到我们在上一个教程中创建的包:
$ roscd learning_tf
1.2启动您的编辑器并将以下代码粘贴到名为src / turtle_tf_listener.cpp的新文件中。
#include<ros/ros.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;
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.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));
turtle_vel.publish(vel_msg);
rate.sleep();
}
return 0;
}
如果在运行时出现错误“Lookup would require extrapolation into the past”,则可以尝试使用此替代代码来调用侦听器:
try {
listener.waitForTransform(destination_frame, original_frame, ros::Time(0), ros::Duration(10.0) );
listener.lookupTransform(destination_frame, original_frame, ros::Time(0), transform);
} catch (tf::TransformException ex) {
ROS_ERROR("%s",ex.what());
}
1.3代码解析
(1) #include <tf/transform_listener.h>
tf软件包提供了一个TransformListener的实现来帮助简化接收转换的任务。 要使用TransformListener,我们需要包含tf / transform_listener.h头文件。 (2) tf::TransformListener listener;
在这里,我们创建一个TransformListener对象。一旦创建了监听器,它就开始接收通过线路进行的tf转换,并缓冲它们长达10秒。TransformListener对象的范围应该保持不变,否则它的缓存将无法填充,几乎每个查询都会失败。一种常见的方法是使TransformListener对象成为类的成员变量。
(3)try
{
listener.lookupTransform("/turtle2", "/turtle1",
ros::Time(0), transform);
}
在这里,真正的工作完成了,我们向侦听器查询特定的转换。我们来看看这四个参数:
我们希望从frame/ turtle1到frame/ turtle2的转换。
我们想要改变的时间。提供ros ::Time(0)只会为我们提供最新的可用转换。
我们存储结果变换的对象。
(4)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的新的线性和角速度,基于它与turtle1的距离和角度。新速度发布在主题“turtle2/ cmd_vel”中,sim将使用它来更新turtle2的移动。
2.1运行侦听器
现在我们创建了代码,让我们先编译它。打开CMakeLists.txt文件,并在底部添加以下行:
add_executable(turtle_tf_listener src/turtle_tf_listener.cpp)
target_link_libraries(turtle_tf_listener ${catkin_LIBRARIES})
进入您的工作空间编译
$ catkin_make
如果一切顺利,你的devel/ lib /learning_tf文件夹中应该有一个名为turtle_tf_listener的二进制文件。
如果是这样,我们已经准备好添加这个演示的启动文件。在您的文本编辑器中,打开名为start_demo.launch的启动文件,然后在<launch>块内合并下面的节点块:
<launch> ... <node pkg="learning_tf" type="turtle_tf_listener" name="listener" /> </launch>
首先,确保你停止了前一篇教程中的启动文件(使用ctrl-c)。现在您已准备好开始您的完整海龟演示:
$ roslaunch learning_tf start_demo.launch
要想看看事情是否有效,只需使用箭头键在第一只乌龟周围驾驶(确保您的终端窗口处于活动状态,而不是您的模拟器窗口),并且您会在第一只乌龟之后看到第一只乌龟! 当turtlesim启动时,您可能会看到: [ERROR] 1253915565.300572000: Frame id /turtle2 does not exist! When trying to transform between /turtle1 and /turtle2.
[ERROR] 1253915565.401172000: Frame id /turtle2 does not exist! When trying to transform between /turtle1 and /turtle2.
发生这种情况是因为我们的听众在收到关于龟2的消息之前正在计算变换,因为它需要一点时间才能在turtlesim中产卵并开始广播一个tf帧。 现在您已准备好转到下一个教程,您将在其中学习如何添加框架(C++)