本教程介绍了如何在tf中使用传感器数据。 传感器数据的一些实际示例是:
- 单声道和立体声相机
- 激光扫描
假设创建了一个新的乌龟,名为turtle3,并且没有很好的里程表,但是有一个高架摄像机跟踪其位置并将其发布为与世界框架有关的geometry_msgs / PointStamped消息。
乌龟1想要知道将turtle3与自身进行比较的地方。
为此,turtle1必须聆听正在发布turtle3姿势的主题,等到准备好转换为所需的帧后再进行操作。 为了简化操作,tf :: MessageFilter类非常有用。 tf :: MessageFilter将使用标头对任何ros消息进行订阅,并将其缓存,直到可以将其转换为目标帧为止。
1.Setup
roslaunch turtle_tf turtle_tf_sensor.launch
接着会出现:
这将使乌龟1自行驱动,乌龟3自行移动。 有一个主题turtle_pose_stamped,其中包含geometry_msgs / PoseStamped数据类型,说明了turtle3在世界框架中的位置。
为了可靠地在turtle1的帧中获取流数据,我们将使用以下代码:
切换行号显示
1 #include "ros/ros.h"
2 #include "tf/transform_listener.h"
3 #include "tf/message_filter.h"
4 #include "message_filters/subscriber.h"
5
6 class PoseDrawer
7 {
8 public:
9 PoseDrawer() : tf_(), target_frame_("turtle1")
10 {
11 point_sub_.subscribe(n_, "turtle_point_stamped", 10);
12 tf_filter_ = new tf::MessageFilter<geometry_msgs::PointStamped>(point_sub_, tf_, target_frame_, 10);
13 tf_filter_->registerCallback( boost::bind(&PoseDrawer::msgCallback, this, _1) );
14 } ;
15
16 private:
17 message_filters::Subscriber<geometry_msgs::PointStamped> point_sub_;
18 tf::TransformListener tf_;
19 tf::MessageFilter<geometry_msgs::PointStamped> * tf_filter_;
20 ros::NodeHandle n_;
21 std::string target_frame_;
22
23 // Callback to register with tf::MessageFilter to be called when transforms are available
24 void msgCallback(const boost::shared_ptr<const geometry_msgs::PointStamped>& point_ptr)
25 {
26 geometry_msgs::PointStamped point_out;
27 try
28 {
29 tf_.transformPoint(target_frame_, *point_ptr, point_out);
30
31 printf("point of turtle 3 in frame of turtle 1 Position(x:%f y:%f z:%f)\n",
32 point_out.point.x,
33 point_out.point.y,
34 point_out.point.z);
35 }
36 catch (tf::TransformException &ex)
37 {
38 printf ("Failure %s\n", ex.what()); //Print exception which was caught
39 }
40 };
41
42 };
43
44
45 int main(int argc, char ** argv)
46 {
47 ros::init(argc, argv, "pose_drawer"); //Init ROS
48 PoseDrawer pd; //Construct class
49 ros::spin(); // Run until interupted
50 };
2.Explanation
2.1Includes
您必须包括tf包中的tf :: MessageFilter标头。 以及以前使用的tf和ROS标头。
1 #include "ros/ros.h"
2 #include "tf/transform_listener.h"
3 #include "tf/message_filter.h"
4 #include "message_filters/subscriber.h"
5
2.2Persistent Data
必须有tf :: TransformListener tf :: MessageFilter的持久实例
切换行号显示
16 private:
17 message_filters::Subscriber<geometry_msgs::PointStamped> point_sub_;
18 tf::TransformListener tf_;
19 tf::MessageFilter<geometry_msgs::PointStamped> * tf_filter_;
20 ros::NodeHandle n_;
21 std::string target_frame_;
2.3Constructor
在启动ros message_filters :: Subscriber时,必须使用该主题进行初始化。 并且tf :: MessageFilter必须使用该Subscriber对象进行初始化。 MessageFilter构造函数中的其他注释参数是target_frame和回调函数。 目标框架是将确保canTransform成功的框架。 回调函数是在数据准备好后将被调用的函数。
切换行号显示
9 PoseDrawer() : tf_(), target_frame_("turtle1")
10 {
11 point_sub_.subscribe(n_, "turtle_point_stamped", 10);
12 tf_filter_ = new tf::MessageFilter<geometry_msgs::PointStamped>(point_sub_, tf_, target_frame_, 10);
13 tf_filter_->registerCallback( boost::bind(&PoseDrawer::msgCallback, this, _1) );
14 } ;
2.4Callback Method
数据准备好后,只需调用transformPose并打印到教程的屏幕即可。
24 void msgCallback(const boost::shared_ptr<const geometry_msgs::PointStamped>& point_ptr)
25 {
26 geometry_msgs::PointStamped point_out;
27 try
28 {
29 tf_.transformPoint(target_frame_, *point_ptr, point_out);
30
31 printf("point of turtle 3 in frame of turtle 1 Position(x:%f y:%f z:%f)\n",
32 point_out.point.x,
33 point_out.point.y,
34 point_out.point.z);
35 }
36 catch (tf::TransformException &ex)
37 {
38 printf ("Failure %s\n", ex.what()); //Print exception which was caught
39 }
40 };
41
42 };
3.Result
point of turtle 3 in frame of turtle 1 Position(x:-0.603264 y:4.276489 z:0.000000)
point of turtle 3 in frame of turtle 1 Position(x:-0.598923 y:4.291888 z:0.000000)
point of turtle 3 in frame of turtle 1 Position(x:-0.594828 y:4.307356 z:0.000000)
point of turtle 3 in frame of turtle 1 Position(x:-0.590981 y:4.322886 z:0.000000)