前言:广播海龟移动中的实时位置。
一、新建功能包
在工作空间中新建learning_tf 功能包,依赖为tf roscpp rospy turtlesim
二、新建cpp文件
新建turtle_tf_broadcaster.cpp文件,文件内容如下:
#include <ros/ros.h> #include <tf/transform_broadcaster.h> #include <turtlesim/Pose.h> std::string turtle_name; void poseCallback(const turtlesim::PoseConstPtr& msg){ static tf::TransformBroadcaster br; tf::Transform transform; transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) ); tf::Quaternion q; q.setRPY(0, 0, msg->theta); transform.setRotation(q); br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name)); } int main(int argc, char** argv){ ros::init(argc, argv, "my_tf_broadcaster"); if (argc != 2){ROS_ERROR("need turtle name as argument"); return -1;}; turtle_name = argv[1]; ros::NodeHandle node; ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback); ros::spin(); return 0; };
----------------------------------------
代码解释:
#include <tf/transform_broadcaster.h>,TF功能包提供了一个广播工具TransformBroadcaster,这个类需要引用<tf/transform_broadcaster.h>头文件
tf::Transform transform; transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) ); tf::Quaternion q; q.setRPY(0, 0, msg->theta);
这里创建了一个变换实例,并将2D位姿信息转化为3D位姿信息。
transform.setRotation(q);
这里设置了旋转
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
使用TransformBroadcaster发送TF变换需要四个参数:
- 变换本身
- 时间戳
- 提供连杆的父框架,这里是world
- 提供连杆的子框架,这里是乌龟本身
三、新建launch文件
新建start_demo.launch文件,文件内容如下:
<launch> <!-- Turtlesim Node--> <node pkg="turtlesim" type="turtlesim_node" name="sim"/> <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/> <!-- Axes --> <param name="scale_linear" value="2" type="double"/> <param name="scale_angular" value="2" type="double"/> <node pkg="learning_tf" type="turtle_tf_broadcaster" args="/turtle1" name="turtle1_tf_broadcaster" /> <node pkg="learning_tf" type="turtle_tf_broadcaster" args="/turtle2" name="turtle2_tf_broadcaster" /> </launch>
运行launch文件
roslaunch learning_tf start_demo.launch
四、查看广播
rosrun tf tf_echo /world /turtle1
-END-