#使用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 
#include 
#include 

   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  
#include  
#include  
#include  

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("spawn");
   turtlesim::Spawn srv;
   add_turtle.call(srv);
 
   ros::Publisher turtle_vel = node.advertise("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文件中粘贴以下代码：

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

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  
#include  
#include  
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文件

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

放置的位置如上图所示 ↑

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)实践四（仅供参考，请勿他用）
理解了就会写了，下期见啦 ~

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

C/C++/C#相关栏目本月热门文章