前言——环境变量那些事儿

编译工作空间后，一定要先设置环境变量，此工作空间下的代码才能有效运行

在工作空间目录下打开终端，输入

source devel/setup.bash

此次source仅对当前terminal有效，若新建的终端都有效,需要sudo gedit ~/.bashrc,在文件最后一行添加source ~/（工作空间目录名)/devel/setup.bash,保存退出

经过本人实验，此方法每次仅能针对一个工作空间有效，当更换工作空间运行代码时，需要打开~/.bashrc重新设置

通过一下命令检查环境变量的设置路径

echo $ROS_PACKAGE_PATH

后续运行代码过程中绝大部分遇到的问题都是因为环境变量设置不正确，血泪教训！！！

一、启动海龟并用键盘控制

打开终端，启动ROS Master

roscore

打开新终端，启动小海龟仿真器

rosrun turtlesim turtlesim_node

打开新终端，启动海龟控制节点

rosrun turtlesim turtle_teleop_key

这样，就可以通过键盘的上下左右控制海龟移动

结果如下

这时，我们可以打开新的终端，查看节点与话题信息

rosnode list

/rosout

/teleop_turtle

/turtlesim

表明此时有三个节点在运行

通过以下命令可以查看节点关系图

rosrun rqt_graph rqt_graph

使用 rostopic echo 命令可以查看在一个话题上发布的数据。它的使用方法如下：

rostopic echo [topic]

例如，我们可以输入：

 rostopic echo /turtle1/cmd_vel

结果如下

现在我们重新查看节点图，发现多了一个订阅者

使用 rostopic list 可以列举出当前系统中的话题：

rostopic list

使用rostopic list -v 可以查看每一个话题的发布者和订阅者的个数。

rostopic list -v

话题上的通信通过节点之间发送消息来完成。对于发布者（turtle_teleop_key）和接收 者（turtlesim_node）的通信，发布者和接收者必须使用类型相同的消息。也就是说，一个 话题的类型是通过在它上面发布的消息的类型来定义的。可以通过：

rostopic type /turtle1/cmd_vel

来查看/turtle1/cmd_vel 的消息类型。你得到的返回类型如下：

geometry_msgs/Twist

我们可以使用 rosmsg 来查看一个消息类型的详细信息：

rosmsg show geometry_msgs/Twist

得到的结果如下

geometry_msgs/Vector3 linear
  float64 x
  float64 y
  float64 z
geometry_msgs/Vector3 angular
  float64 x
  float64 y
  float64 z

我们可以基于此发布信息，用具体的数值控制小海龟移动。

下面来尝试按照记忆路线控制小海龟移动

rosbag record -a -O cmd_record

保留之前的终端窗口，再创建一个新窗口，输入：~$ rosbag record -a -O cmd_record 开始记录小海龟路径，出现：

将光标放在启动键盘控制的终端，控制小海龟移动，此时已经开始记录小海龟的路径信息，记录完成后将光标放在开启记录路径的终端 按下< ctrl > + < C > 停止记录，主文件夹会生成一个.bag文件就是我们的记录文件：

将之前的终端全部关掉，重新按照步骤打开小海龟控制窗口后，打开一个新的终端输入：

~$ rosbag play cmd_record.bag

小海龟就开始按照之前记录的路径开始移动了～～～

二、控制海龟画圆

1.用命令行

打开终端，启动ROS Master

roscore

打开新终端，启动小海龟仿真器

rosrun turtlesim turtlesim_node

打开新终端，创建发布者

rostopic pub -r 10 /turtle1/cmd_vel geometry_msgs/Twist -- '[1.8, 0.0, 0.0]' '[0.0, 0.0, 1.8]'

或

rostopic pub -r 10 /turtle1/cmd_vel geometry_msgs/Twist "linear:
x: 1.0
y: 0.0
z: 0.0
angular:
x: 0.0
y: 0.0
z: 1.0"

其中linear是线速度，angular是角速度

结果如下：

创建第二只海龟需要使用/spawn服务

我们先来查看/spawn的信息

 rosservice info /spawn

输入以下命令

rosservice call /spawn "x: 2.0
y: 2.0
theta: 0.0
name: 'turtle2'" 

这就在 x=2, y=2, 角度=0 的地方，创建了一个名字是 turtle2 的小海龟如下图：

2.通过ROS的话题通信机制

（1）控制海龟画圈

本人功能包结构如下：

test_pkg

->include

->launch

-->start_talker.launch

->src

-->talker.cpp

-->listener.cpp

->CmakeLists.txt

->package.xml

在创建功能包时，已经安装了所需要的依赖，如果后续发现仍需要安装新的依赖，可以在功能包路径下用命令行安装，并自行修改CmakeLists.txt与package.xml

sudo apt-get install [软件包名]

例如：

sudo apt-get install ros-kinetic-PACKAGE

回到正题，我们需要为海龟画圈创建发布者、订阅者，为了方便启动，最好再写一个launch文件

在发布者中，我们需要在提供模板（详见古月居相关课程）的基础上，添加消息类型对应的头文件依赖（include……）、创建一个自己的发布者、并初始化对应的消息类型数据

创建一个Publisher，发布名为/turtle1/cmd_vel的topic，消息类型为geometry_msgs::Twist

ros::Publisher chatter_pub = n.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 10);

然后设置linear.x为1，angular.z为0.5

在订阅者中，我们需要在模板的基础上，创建一个自己的订阅者，并修改打印信息参数

创建一个Subscriber，订阅名为/turtle1/pose的topic，注册回调函数chatterCallback

ros::Subscriber sub = n.subscribe("/turtle1/pose", 100, chatterCallback);

在launch文件中，我们需要启动海龟仿真器、自己的发布者与订阅者

效果如下图示所示

（2）同时控制两只海龟

让第一只海龟画圈，第二只海龟以圆弧轨迹从（5,5）运动到（10,10）

本人功能包结构如下：

test_pkg

->include

->launch

-->start.launch

->src

-->talker.cpp

-->talker1.cpp

-->listener.cpp

-->add_turtle1.cpp

->CmakeLists.txt

->package.xml

首先，我们需要在代码中添加第二只海龟，这通过调用/spawn服务来实现，具体参照古月居服务端的编程实现。我们需要设置生成海龟的初始位置、运动速度和它的名字“turtle2”

在发布者中，我们需要在提供模板的基础上，添加消息类型对应的头文件依赖（include……）、创建一个自己的发布者、并初始化对应的消息类型数据

在订阅者中，我们需要在模板的基础上，创建一个自己的订阅者，并修改打印信息参数

创建一个Subscriber，订阅名为/turtle1/pose的topic，注册回调函数chatterCallback

在launch文件中，我们需要启动海龟仿真器、add_turtle节点、自己的发布者与订阅者

效果如下图示所示

（3）使用tf变换进行海龟跟随试验

我们的目的是，召唤出两只海龟，用键盘控制第一只海龟的移动，另一只海龟自动跟随。当然，你也可以结合上文控制海龟画圆的内容自行设计轨迹。

想要实现目标，首先在launch文件中设置turtlesim_node、turtle_teleop_key两个节点的启动，这里不做过多赘述。

然后，我们就可以关注两只海龟的控制了。

在发布者中，我们需要接收两只海龟的位姿信息，包括坐标、角度、线速度、角速度。

其次，需要定义tf广播器，并设置当前小乌龟相对于世界坐标系的坐标变换，设置子坐标系在父坐标系下的位置信息，并发布坐标变换。

// 根据乌龟当前的位姿 设置当前小乌龟相对于世界坐标系的坐标变换
    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);
 
    // 发布坐标变换 四个参数分别为 tf转换 时间戳 目标坐标系 待转换坐标系
    br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));

ros::NodeHandle node;
ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);

在订阅者中调用/spawn服务，在（5,5）生成第二只海龟；定义tf监听器，发布第二只海龟的速度；查找turtle2与turtle1的坐标变换；根据turtle1和turtle2之间的坐标变换计算turtle2需要运动的线速度和角速度，并发布速度控制指令使turtle2向turtle1移动

// transform代表两个坐标系之间的转换关系
        geometry_msgs::Twist vel_msg;

        vel_msg.angular.z = 9.0 * atan2(transform.getOrigin().y(),transform.getOrigin().x());
        // 计算角速度 使用getOrigin获得的是跟随乌龟在主控乌龟坐标系下的位置信息
        // atan2函数返回以弧度表示的y/x的反正切
        vel_msg.linear.x = 1.5 * sqrt(pow(transform.getOrigin().x(), 2) + pow(transform.getOrigin().y(), 2));
        // 计算线速度
        turtle_vel.publish(vel_msg);

然后，在launch文件中添加监听器与两只海龟的广播器。

本人功能包结构如下

test_follow

->include

->launch

-->start.launch

->src

-->my_tf_listener.cpp

-->my_tf_broadcaster.cpp

->CmakeLists.txt

->package.xml

运行效果如下：

可以看出，跟踪效果非常好(不要贴贴😜)

用rosnode list命令查看运行的节点

用rostopic list命令查看存在的话题

画出节点树

rosrun rqt_graph rqt_graph

通过 一下命令录制数据，再现记忆路线，来看看跟踪效果

rosbag record -a -O cmd_record

~$ rosbag play cmd_record.bag