#使用TF实现海龟机器人跟随

昨天粗略地讲解了一会儿TF变换，用的是ROS系统中自带的功能包实现小海龟跟随的功能（具体见→初识TF变换）。今天我们将用自己编写节点的方式实现小海龟跟随的功能，并且，在之前的基础上，建立一个新的参考坐标系。

1、在工作空间的src目录下新建功能包

lzw08@ubuntu:~$ cd ros_ws/src
lzw08@ubuntu:~/ros_ws/src$ catkin_create_pkg turtle_follow std_msgs roscpp rospy tf geometry_msgs

说明：turtle_follow 为功能名称，std_msgs、roscpp、rospy、tf、geometry_msgs都是此功能包的依赖项。

2、进入功能包的src目录下新建.cpp文件

lzw08@ubuntu:~/ros_ws/src$ cd turtle_follow/src

1）turtle_broadcaster.cpp

lzw08@ubuntu:~/ros_ws/src/turtle_follow/src$ touch turtle_broadcaster.cpp
lzw08@ubuntu:~/ros_ws/src/turtle_follow/src$ gedit turtle_broadcaster.cpp

#include<ros/ros.h>
#include<tf/transform_broadcaster.h>
#include<turtlesim/Pose.h> using namespace std; 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, "turtle_tf_listener");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;}

2）turtle_listener.cpp

lzw08@ubuntu:~/ros_ws/src/turtle_follow/src$ touch turtle_listener.cpp
lzw08@ubuntu:~/ros_ws/src/turtle_follow/src$ gedit turtle_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, "turtle_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", 2);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;}

3、修改编译规则CMakeList.txt

lzw08@ubuntu:~/ros_ws/src/turtle_follow/src$ cd ..
lzw08@ubuntu:~/ros_ws/src/turtle_follow$ gedit CMakeLists.txt

具体修改了138、139行和152~158行。以下供复制：

add_executable(turtle_broadcaster src/turtle_broadcaster.cpp)
add_executable(turtle_listener src/turtle_listener.cpp)

  target_link_libraries(turtle_broadcaster${catkin_LIBRARIES})target_link_libraries(turtle_listener${catkin_LIBRARIES})

4、回到工作空间下编译

lzw08@ubuntu:~/ros_ws/src/turtle_follow$ cd ~/ros_ws
lzw08@ubuntu:~/ros_ws$ catkin_make

编译通过后我们不再通过打开一个终端运行主节点、打开一个终端运行turtle_broadcaster节点、打开一个新终端运行turtle_listener节点的方式运行，换另外一种高级些的（见步骤5）。

5、新建.launch 文件

刚才编译的时候我们回到了工作空间，现在得重新进入turtle_follow功能包。

lzw08@ubuntu:~/ros_ws$ cd src/turtle_follow/

新建launch目录用以存放.launch文件

lzw08@ubuntu:~/ros_ws/src/turtle_follow$ mkdir launch           //新建launch目录
lzw08@ubuntu:~/ros_ws/src/turtle_follow$ cd launch/                //进入launch目录
lzw08@ubuntu:~/ros_ws/src/turtle_follow/launch$ touch follow.launch        //新建follow.launch文件
lzw08@ubuntu:~/ros_ws/src/turtle_follow/launch$ gedit follow.launch        //打开follow.launch文件

在follow.launch文件中粘贴以下代码：

<launch>
<!-- 海⻳仿真器 -->
<node pkg="turtlesim" type="turtlesim_node" name="sim"/>
<!-- 键盘控制 -->
<node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/> <!-- 两只海⻳的tf⼴播 -->
<node pkg="turtle_follow" type="turtle_broadcaster" args="/turtle1" name="turtle1_tf_broadcaster" />
<node pkg="turtle_follow" type="turtle_broadcaster" args="/turtle2" name="turtle2_tf_broadcaster" /><!-- 监听tf⼴播，并且控制turtle2移动 -->
<node pkg="turtle_follow" type="turtle_listener" name="listener" /></launch>

保存并关闭即可，不需要编译！

6、运行follow.launch 文件

回到工作空间下source一下，就可以运行了

lzw08@ubuntu:~/ros_ws/src/turtle_follow/launch$ cd ~/ros_ws
lzw08@ubuntu:~/ros_ws$ source devel/setup.bash
lzw08@ubuntu:~/ros_ws$ roslaunch turtle_follow follow.launch

可见，运行效果跟昨天初识TF变换那里是一样的，所不同的就是，昨天那个是系统自带的，今天这个是新建节点实现的，到这里今天的任务就实现一半了。

7、新建参考坐标系carrot1

lzw08@ubuntu:~/ros_ws$ cd src/turtle_follow/src/
lzw08@ubuntu:~/ros_ws/src/turtle_follow/src$ touch turtle_tf.cpp
lzw08@ubuntu:~/ros_ws/src/turtle_follow/src$ gedit turtle_tf.cpp

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <math.h>
int main(int argc, char** argv)
{ ros::init(argc, argv, "turtle_tf_listener"); ros::NodeHandle node;tf::TransformBroadcaster br; tf::Transform transform;ros::Rate rate(10.0);while (node.ok()){//创建transform，从⽗系turtle1到⼦系carrot1，carrot1离turtle1 两⽶远transform.setOrigin( tf::Vector3(2.0, 2.0 , 0.0) );transform.setRotation( tf::Quaternion(0, 0, 0, 1) );br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "turtle1", "carrot1"));rate.sleep();}return 0;
}

8、修改编译规则CMakeList.txt及follow.launch 文件等

1）CMakeList.txt

lzw08@ubuntu:~/ros_ws/src/turtle_follow/src$ cd ..
lzw08@ubuntu:~/ros_ws/src/turtle_follow$ gedit CMakeLists.txt

今天写了三个.cpp文件所以要有三个add_executable和三个target_link_libraries：

add_executable(turtle_tf src/turtle_tf.cpp)

target_link_libraries(turtle_tf${catkin_LIBRARIES})

2）follow.launch

lzw08@ubuntu:~/ros_ws/src/turtle_follow$ cd launch/       //进入launch目录
lzw08@ubuntu:~/ros_ws/src/turtle_follow/launch$ gedit follow.launch          //打开follow.launch文件

把下面这句代码加进去就行了：

<node pkg="turtle_follow" type="turtle_tf"  name="broadcaster_frame" />

放置的位置如上图所示 ↑

3）turtle_listener.cpp

如下图所示，将第26、27行的“turtle1”都改成“carrot1”，然后保存关闭即可。（不要纠结为什么你的turtle1和carrot1没有颜色！高亮只是方便你们看清并修改）
修改前：

修改后：

9、回到工作空间下编译

lzw08@ubuntu:~/ros_ws/src/turtle_follow/launch$ cd ~/ros_ws
lzw08@ubuntu:~/ros_ws$ catkin_make

像这样编译通过就可以运行了。

10、运行

lzw08@ubuntu:~/ros_ws$ roslaunch turtle_follow follow.launch

因为有了参考坐标系的存在，第二只小海龟不再与第一只小海龟重合了，它们之间有一点的距离，不过还是跟随的。

11、rviz

12、总结

如果出现下图所示错误：

运行下面这句代码就可以解决了：

source devel/setup.bash

到这就算所有工作都做完了，对于实践四报告该如何写，我觉得老师给的模板顺序可能有点不太好，所以我浅改一下：

1、创建使小海龟移动跟随目标的功能包。
2、新建 tf 广播*.cpp
3、创建监听*.cpp
4、修改CMakeLists.txt文件内容
5、修改package.xml文件内容
6、建立固定参考系*.cpp
7、修改CMakeLists.txt文件内容
8、在功能包下创建并编写 launch 文件
9、运行程序进行测试
10、图形化工具查看节点、话题等信息

附本人实践报告 → 机器人操作系统(ROS)实践四（仅供参考，请勿他用）
理解了就会写了，下期见啦 ~