假设一只新的乌龟 命名为 乌龟3 , 它没有好的里程计(也就是说不指定它的具体坐标),但是现在有一个上方的相机 跟踪 它的位置并且发布 相对与 世界坐标系 下的 位置信息 ( PointStamped 类型的 topic)
乌龟1 想要知道 乌龟3 相对自己的位置
为此,turtle1必须监听正在发布turtle3位置的topic,直到准备好转换为期望坐标系下,然后进行操作。
做这件事的话,使用 tf2_ros::MessageFilter 类 非常有用
tf2_ros :: MessageFilter将使用标头对任何ros消息进行订阅,并将其缓存,直到可以将其转换为期望坐标系下为止。
2、实现代码
|
|
class PoseDrawer | |
{ | |
public: | |
PoseDrawer() : | |
tf2_(buffer_), target_frame_("turtle1"), | |
tf2_filter_(point_sub_, buffer_, target_frame_, 10, 0) | |
{ | |
point_sub_.subscribe(n_, "turtle_point_stamped", 10); | |
tf2_filter_.registerCallback( boost::bind(&PoseDrawer::msgCallback, this, _1) ); | |
} | |
// Callback to register with tf2_ros::MessageFilter to be called when transforms are available | |
void msgCallback(const geometry_msgs::PointStampedConstPtr& point_ptr) | |
{ | |
geometry_msgs::PointStamped point_out; | |
try | |
{ | |
buffer_.transform(*point_ptr, point_out, target_frame_); | |
ROS_INFO("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 (tf2::TransformException &ex) | |
{ | |
ROS_WARN("Failure %s\n", ex.what()); //Print exception which was caught | |
} | |
} | |
private: | |
std::string target_frame_; | |
tf2_ros::Buffer buffer_; | |
tf2_ros::TransformListener tf2_; | |
ros::NodeHandle n_; | |
message_filters::Subscriber |
|
tf2_ros::MessageFilter |
|
}; | |
int main(int argc, char ** argv) | |
{ | |
ros::init(argc, argv, "pose_drawer"); //Init ROS | |
PoseDrawer pd; //Construct class | |
ros::spin(); // Run until interupted | |
return 0; | |
}; | |
3、代码解释
=============================================================
|
|
首先时包含 tf2_ros::MessageFilter 的头文件 和其它头文件
===============================================================
private: |
|
std::string target_frame_; | |
tf2_ros::Buffer buffer_; | |
tf2_ros::TransformListener tf2_; | |
ros::NodeHandle n_; | |
message_filters::Subscriber |
|
tf2_ros::MessageFilter |
|
类的私有变量
tf2_ros::Buffer
tf2_ros::TransformListener
tf2_ros::MessageFilter
这三个要在构造函数时初始化 使数据一直被保持
====================================================================
PoseDrawer() : |
|
tf2_(buffer_), target_frame_("turtle1"), | |
tf2_filter_(point_sub_, buffer_, target_frame_, 10, 0) | |
{ | |
point_sub_.subscribe(n_, "turtle_point_stamped", 10); | |
tf2_filter_.registerCallback( boost::bind(&PoseDrawer::msgCallback, this, _1) ); | |
} | |
启动ros message_filters :: Subscriber时必须使用该主题进行初始化。 并且tf2_ros :: MessageFilter必须使用该Subscriber对象进行初始化。 MessageFilter构造函数中需要注意的其他参数是target_frame和回调函数。 目标框架是确保canTransform成功执行的框架。 回调函数是在数据准备好后将被调用的函数。
=======================================================================
// Callback to register with tf2_ros::MessageFilter to be called when transforms are available |
|
void msgCallback(const geometry_msgs::PointStampedConstPtr& point_ptr) | |
{ | |
geometry_msgs::PointStamped point_out; | |
try | |
{ | |
buffer_.transform(*point_ptr, point_out, target_frame_); | |
ROS_INFO("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 (tf2::TransformException &ex) | |
{ | |
ROS_WARN("Failure %s\n", ex.what()); //Print exception which was caught | |
} | |
} | |
tf2_ros::MessageFilter 的 回调函数
buffer_.transform(*point_ptr, point_out, target_frame_);
通过这个函数(就是tf2_ros::TransformListener的一个功能函数) 将 在 世界坐标系的 点 转化为 期望坐标系 下的点
4、 疑问
上面通过 tf2_ros::MessageFilter 的 方式 和 直接订阅 位置信息 然后 通过tf2_ros::TransformListener的transform功能函数去结算 这两者有什么区别呢?
第三方账号登入
QQ 微博 微信