乌龟跟随实例-TF坐标变换

 以下创建的代码是基于https://blog.csdn.net/qq_34911636/article/details/100103448 这篇文章创建的工作空间中开发

1. 创建工作空间

https://blog.csdn.net/qq_34911636/article/details/100103448 这篇文章创建的工作空间，是创建基于

catkin_create_pkg  std_msgs rospy roscpp 的工作空间，乌龟跟随实例的依赖包是基于tf和turtlesim所以

在创建工作空间时需为catkin_create_pkg learning_tf tf roscpp rospy turtlesim，其中tf roscpp rospy turtlesim

为依赖项，注：该乌龟跟随实例的工作空间的文件夹名称为catkin_tf

2. 创建TF广播器，在src文件夹下面创建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;
}；

3.创建TF监听器，在src文件夹下面创建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文件最后添加编译项

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.打开终端编译

$ cd ~/catkin_ws/
$ catkin_make

如果编译时有出现如下错误：

“CMakeFiles/turtle_tf_listener.dir/src/turtle_tf_listener.cpp.o：在函数‘main’中：
turtle_tf_listener.cpp:(.text+0x2a4)：对‘tf::Transformer::DEFAULT_CACHE_TIME’未定义的引用
turtle_tf_listener.cpp:(.text+0x2e0)：对‘tf::TransformListener::TransformListener(ros::Duration, bool)’未定义的引用
turtle_tf_listener.cpp:(.text+0x432)：对‘tf::Transformer::waitForTransform(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&, ros::Duration const&, ros::Duration const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >*) const’未定义的引用”

该错误的原因是CMakeLists.txt未修改和添加依赖
修改内容和依赖添加如下：（注：以下修改内容并不是都完全按照以下修改，一般没有自定义消息，只需要修改find_package即可）

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  message_generation
  tf
)

generate_messages(
   DEPENDENCIES
  std_msgs
 )

catkin_package(
  INCLUDE_DIRS include
  LIBRARIES learning_tf 
  CATKIN_DEPENDS roscpp rospy std_msgs
  DEPENDS system_lib
)

include_directories(
 include
  ${catkin_INCLUDE_DIRS}
)

6.创建launch文件

a.首先进入到工作空间的src目录，然后执行命令catkin_create_pkg mylaunch （mylaunch为包名，需要自己设定）

    这边我们命名为tflaunch

b.执行rospack profile，更新现有包

c.进入tflaunch目录，就可以在此目录创建launch文件了,我们创建一个start_tf.launch的文件

start_tf.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>

7.切换到catkin_tf路径下面：cd ~/catkin_tf，其中catkin_tf为我们创建乌龟跟随实例工作空间的文件夹

输入命令：roslaunch tflaunch start_tf.launch，tflaunch为launch文件夹

这时候即可看到一只乌龟朝着另外一只乌龟运动

若输入命令：roslaunch tflaunch start_tf.launch

出现以下的错误提示：

则需输入命令：source  devel/setup.bash 设置工作空间的环境变量即可

8.打开一个新的终端，在终端中输入命令：rosrun turtlesim turtle_teleop_key

即可通过键盘控制第一个乌龟，第二个乌龟跟随第一个乌龟运动