本教程介绍了如何将传感器数据与tf一起使用。一些传感器数据的实际例子是:
- 单目摄像头和双目摄像头
- 激光雷达
假设创建了一只名为turtle3的新龟,但它没有里程信息;
但有一个高架摄像机跟踪了其位置,并将其位置作为与世界坐标系相关的geometry_msgs / PointStamped消息发布出来。
定义的geometry_msgs / PointStamped消息类型。
Turtle 1如果想知道turtle3与自身的比较呢?
要做到这一点,turtle1必须收听正在发布的关于turtle3姿势话题,等待转换到所需的坐标已准备好,然后执行其操作。
为了使这更容易,tf :: MessageFilter类非常有用。
tf :: MessageFilter将订阅任何ros消息并对其进行缓存,直到可以将其转换为目标坐标。
1 Setup
roslaunch turtle_tf turtle_tf_sensor.launch
这将会使得龟1和龟3自己移动。
话题为turtle_point_stamped,其中包含一个geometry_msgs / PoseStamped数据类型,说明了turtle3在世界框架中的位置。
为了可靠地获取turtle1坐标中的流数据,我们使用以下代码:
#include "ros/ros.h"
#include "tf/transform_listener.h"
#include "tf/message_filter.h"
#include "message_filters/subscriber.h"
class PoseDrawer
{
public:
PoseDrawer() : tf_(), target_frame_("turtle1")
{
point_sub_.subscribe(n_, "turtle_point_stamped", 10); //节点,话题,缓冲大小
tf_filter_ = new tf::MessageFilter<geometry_msgs::PointStamped>(point_sub_, tf_, target_frame_, 10);
tf_filter_->registerCallback( boost::bind(&PoseDrawer::msgCallback, this, _1) );
//this->msgCallback(x)
} ;
private:
message_filters::Subscriber<geometry_msgs::PointStamped> point_sub_;
tf::TransformListener tf_;
tf::MessageFilter<geometry_msgs::PointStamped> * tf_filter_;
ros::NodeHandle n_;
std::string target_frame_;
// Callback to register with tf::MessageFilter to be called when transforms are available
void msgCallback(const boost::shared_ptr<const geometry_msgs::PointStamped>& point_ptr)
{
geometry_msgs::PointStamped point_out;
try
{
tf_.transformPoint(target_frame_, *point_ptr, point_out);
printf("point of turtle 3 in frame of turtle 1 Position(x:%f y:%f z:%f)\n",
point_out.point.x,
point_out.point.y,
point_out.point.z);
}
catch (tf::TransformException &ex)
{
printf ("Failure %s\n", ex.what()); //Print exception which was caught
}
};
};
int main(int argc, char ** argv)
{
ros::init(argc, argv, "pose_drawer"); //Init ROS
PoseDrawer pd; //Construct class
ros::spin(); // Run until interupted
};
2 说明
2.1 包括
您必须包含tf包中的tf :: MessageFilter标头。就行之前使用的tf和ROS头文件。
#include "ros/ros.h"
#include "tf/transform_listener.h"
#include "tf/message_filter.h"
#include "message_filters/subscriber.h"
2.2 永久数据
There need to be persistent instances of tf::TransformListener tf::MessageFilter
private:
message_filters::Subscriber<geometry_msgs::PointStamped> point_sub_;
tf::TransformListener tf_;
tf::MessageFilter<geometry_msgs::PointStamped> * tf_filter_;
ros::NodeHandle n_;
std::string target_frame_;
2.3 构造函数
启动时,必须使用话题初始化ros message_filters :: Subscriber。并且必须使用该Subscriber object初始化tf :: MessageFilter。
The other arguments of note in the MessageFilter constructor are the target_frame and callback function.
The target frame is the frame into which it will make sure canTransform will succeed。
回调函数是在数据准备好时调用的函数。
PoseDrawer() : tf_(), target_frame_("turtle1")
{
point_sub_.subscribe(n_, "turtle_point_stamped", 10);
tf_filter_ = new tf::MessageFilter<geometry_msgs::PointStamped>(point_sub_, tf_, target_frame_, 10);
tf_filter_->registerCallback( boost::bind(&PoseDrawer::msgCallback, this, _1) );
} ;
注意:注册了一个回调函数。而bind把成员函数msgCallback绑定到对象上。
2.4 回调方法
Once the data is ready, just call transformPose and print to screen for the tutorial.
void msgCallback(const boost::shared_ptr<const geometry_msgs::PointStamped>& point_ptr)
{
geometry_msgs::PointStamped point_out;
try
{
tf_.transformPoint(target_frame_, *point_ptr, point_out); //这个是重点
printf("point of turtle 3 in frame of turtle 1 Position(x:%f y:%f z:%f)\n",
point_out.point.x,
point_out.point.y,
point_out.point.z);
}
catch (tf::TransformException &ex)
{
printf ("Failure %s\n", ex.what()); //Print exception which was caught
}
};
};
3 结果
如果它正在运行,你应该看到像这样的流数据。
海龟3在龟的框架中的位置1位置(x:-0.603264 y:4.276489 z:0.000000)
海龟3在龟的框架中的位置1位置(x:-0.598923 y:4.291888 z:0.000000)
海龟3在龟的框架中的位置1位置(x:-0.594828 y:4.307356 z:0.000000)
海龟3在龟的框架中的位置1位置(x:-0.590981 y:4.322886 z:0.000000)