TF坐标变换是导航和定位中基础的知识,在学习ROS中,很快就接触到了。下面记录一下小乌龟例程中的TF 功能包。
首先要下载turtle_tf:
sudo apt-get install ros-melodic-turtle-tf
需要对应相应的版本。安装之后,可以使用下面命令来运行
roslaunch turtle_tf turtle_tf_demo.launch
再打开一个节点,来控制小乌龟的运动:
rosrun turtlesim turtle_teleop_key
可以使用下面命令来查看TF树:
rosrun tf view_frames
这个命令会把TF树保存在一个pdf文件中,我们可以使用如下命令打开:
evince frames.pdf
使用如下命令查看乌龟坐标系之间的变换关系:
rosrun tf tf_echo turtle1 turtle2
以上都是下载了功能包之后可以进行的可视化操作。
下面进行代码编写:
1.新建功能包,依赖项为std_msgs rospy roscpp tf turtlesim geometry_msgs
catkin_create_pkg learning_tf std_msgs rospy roscpp tf turtlesim
本人在刚开始的没有添加geometry_msgs,后来在xml文件和CMakeList.txt中手动添加.
2.返回工作空间,进行编译:
cd ..
catkin_make
3.在src文件价中add两个cpp文件(1)turtle_tf_broadcaster.cpp (2)turtle_tf_listener.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)
{
//TF 广播器
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];
//订阅乌龟的pose信息
ros::NodeHandle node;
ros::Subscriber sub = node.subscribe(turtle_name+"/pose",10,&poseCallback);
ros::spin();
return 0;
}
//第二个文件
//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.waitForTransform("/turtle2","/turtle1",ros::Time(0),ros::Duration(3.0));
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;
}
4.修改CMakeLists.txt文件
#首先是功能包支持
find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
std_msgs
geometry_msgs
tf
turtlesim
)
#编译选项
include_directories(
# include
${catkin_INCLUDE_DIRS}
)
add_executable(turtle_tf_broadcaster src/turtle_tf_broadcaster.cpp)
target_link_libraries(turtle_tf_broadcaster ${catkin_LIBRARIES})
add_executable(turtle_tf_listener src/turtle_tf_listener.cpp)
target_link_libraries(turtle_tf_listener ${catkin_LIBRARIES})
5.修改XML文件,主要是包含以下文件:
<buildtool_depend>catkin</buildtool_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>tf</build_depend>
<build_depend>turtlesim</build_depend>
<build_export_depend>geometry_msgs</build_export_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>rospy</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<build_export_depend>tf</build_export_depend>
<build_export_depend>turtlesim</build_export_depend>
<exec_depend>geometry_msgs</exec_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>rospy</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>tf</exec_depend>
<exec_depend>turtlesim</exec_depend>
6. 回到工作空间,编译
catkin_make
7.编写launch启动文件:start_demo_with_listener.launch
<launch>
<!-- 海龟仿真器 -->
<node pkg="turtlesim" type="turtlesim_node" name="sim"/>
<!-- 键盘控制 -->
<node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>
<!-- 两只海龟的TF广播 -->
<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"/>
<!-- 监听TF广播,并且控制turtle2移动 -->
<node pkg="learning_tf" type="turtle_tf_listener" name="listener"/>
</launch>
8. 编译成功之后,就可以运行这个例程了。
roslaunch learning_tf start_demo_with_listener.launch
之后就能看到用方向键控制两只海龟相随运动了。