# MRS-TB3

**Repository Path**: Mutil-Unmanned-System/mrs-tb3

## Basic Information

- **Project Name**: MRS-TB3
- **Description**: 基于turtlebot3的多机器人编队demo
- **Primary Language**: Unknown
- **License**: Not specified
- **Default Branch**: master
- **Homepage**: None
- **GVP Project**: No

## Statistics

- **Stars**: 10
- **Forks**: 0
- **Created**: 2022-07-17
- **Last Updated**: 2025-09-20

## Categories & Tags

**Categories**: Uncategorized

**Tags**: None

## README

[TOC]

# 1 介绍

## 1.1 注意事项

**代码运行环境为:** Ubuntu20.4 + ROS N版

<font color="red">**代码文件命令格式：**</font>

```HTML
姓名_功能_版本
ZhouZheng_Mas_Prescribed_time_V1
```

## 1.2 文件结构

**sensor_interface** 目录用于存放传感器接口

**robot** turtlebot3机器人接口

**demo** 目录为存放演示程序的目录

**Reference** 目录为存放参考资料的地方

## 1.3 分支结构

**master** 工控机的分支，里面有实物代码和实物传感器驱动以及仿真代码；

**feature/tb3_pratice_demo** 工控机的实验分支，里面有实物代码和实物传感器驱动，后续不使用了，直接用master分支；

**feature/tb3_gazebo**  自己电脑仿真分支，里面有基本的小车模型和一些gazebo仿真代码；

**feature/target_detection** 用于目标检测的分支，目前没用，后续传代码，非必要不要切换到这个分支。

# 2 在仿真环境中启动turtlebot3

## 2.1 启动turtlebot3节点

1）鱼香一键安装ros，注意对应Ubuntu版本，ubuntu20对应Noetic

```bash
wget http://fishros.com/install -O fishros && . fishros
```

2）安装依赖项

```shell
sudo apt-get install ros-noetic-joy  ros-noetic-teleop-twist-keyboard ros-noetic-serial
```
3)先创建ROS工作空间，输入指令 

```bash
# 创建一个新的ROS工作空间
mkdir -p ~/catkin_ws/src
cd ~/catkin_ws/src
# 初始化工作空间
catkin_init_workspace
# 构建工作空间
cd ~/catkin_ws
catkin_make
# 设置环境变量以便在任何新的终端中都能找到ROS工作空间
source devel/setup.bash
```

```bash
# 有需要的话，可以自己创建一个新的ROS功能包，用git clone的功能包一般就够用了
catkin_create_pkg my_robot roscpp std_msgs
```

现在你应该可以开始在你的`my_robot`包中编写代码了。 

4）从项目仓库下载turtlebot3代码包

```shell
cd ~/catkin_ws/src/
git clone https://gitee.com/Mutil-Unmanned-System/mrs-tb3.git
cd mrs-tb3 && git checkout feature/tb3_gazebo #这个命令是切换到feature/tb3_gazebo分支，用于仿真的
cd ~/catkin_ws && catkin_make
```

5）新建一个终端启动仿真

```shell
source devel/setup.bash
export TURTLEBOT3_MODEL=burger
roslaunch turtlebot3_fake turtlebot3_fake.launch
```

6）启动键盘teleop节点控制turtlebot3移动。

```shell
source devel/setup.bash
export TURTLEBOT3_MODEL=burger
roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch
```

接下来就可以看到如下效果(如图 reference/image/1-1.png):

![](reference/image/1-1.png)

## 2.2 使用 Gazebo 仿真环境启动turtlebot3

1)第一次使用Turtlebot3的gazebo仿真，需要把Turtlebot3的模型文件，复制到gazebo的模型目录里

```shell
mkdir -p ~/.gazebo/models/
cp -r  ~/catkin_ws/src/mrs-tb3/robot/turtlebot3_simulations/turtlebot3_gazebo/worlds/ ~/.gazebo/models/
```

2)启动仿真。设置模型参数，指定使用那种机器人型号. 启动世界地图，默认的空白地图环境中加载Turtlebot3机器人

```shell
source devel/setup.bash
export TURTLEBOT3_MODEL=burger
roslaunch turtlebot3_gazebo turtlebot3_empty_world.launch
```

3)新建一个终端，用键盘控制turtlebot3

```shell
source devel/setup.bash
export TURTLEBOT3_MODEL=burger
roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch
```

4)通过键盘上的按键wsadx就可以遥控仿真环境中的turtlebot3机器人了 (如图 reference/image/1-2.png):

![](reference/image/1-2.png)

5)或者为了方便，将其直接添加到环境变量中：

```shell
echo "source devel/setup.bash" >> ~/.bashrc
echo "export TURTLEBOT3_MODEL=burger" >> ~/.bashrc
```

## 2.3 Gazebo环境下多机器人编队

### 2.3.1 基于TF实现多机器人编队

基于TF树的方式实现多机器人编队，**tb3_leaderfollower_with_tf** 节点接收1-4号机器人的状态数据，并发布到TF 上，0号机器人直接由键盘控制。在主循环中获取机器人TF坐标数据，计算控制量并发布到 **/tb3_x/cmd_vel** 上控制机器人移动。

1)启动仿真。使用默认的空白地图环境中加载5个Turlebot3机器人，并进行简单线性跟随。

```shell
roslaunch tb3_leaderfollower_with_tf tb3_leaderfollower_with_tf.launch
```

2)启动键盘，用来控制领航者turtlebot3机器人。

```shell
ROS_NAMESPACE=tb3_0 rosrun turtlebot3_teleop turtlebot3_teleop_key
```

3)通过键盘上的按键wsadx就可以遥控仿真环境中的turtlebot3机器人了 (如视频 reference/image/1-3.gif)

![](reference/image/1-3.gif)

### 2.3.2 基于Topic通信方式实现多机器人编队

基于Tpoic话题的方式实现多机器人编队，**tb3_leaderfollower_with_topic** 节点接收1-4号机器人的状态数据，0号机器人直接由键盘控制。

1)启动仿真。使用默认的空白地图环境中加载5个Turlebot3机器人，并进行简单线性跟随。

```shell
roslaunch tb3_leaderfollower_with_topic tb3_leaderfollower_with_topic.launch
```

2)启动键盘，用来控制领航者turtlebot3机器人。

```shell
ROS_NAMESPACE=tb3_0 rosrun turtlebot3_teleop turtlebot3_teleop_key
```

3)通过键盘上的按键wsadx就可以遥控仿真环境中的turtlebot3机器人了 (效果与TF 方式类同)

## 2.4 自定义仿真代码

1）函数说明

设备：虚拟机ubuntu20.04

参考代码路径：/home/ubuntu/catkin_ws/src/mrs-tb3/demo/tb3_gazebo/src/tb3_gazebo_luo_2024.cpp 

```shell
变量说明：
N 无人车的数量，根据实验设定，该代码为4，如果要用5辆车，记得取消注释：
// sub4 = node.subscribe("/tb3_4/odom", 10, &multiThreadListener::poseCallback4,this);
p 无人车的位置信息，2N*1的列向量。
v 无人车的速度信息，2N*1的列向量，该代码注释掉了速度的获取，详见multiThreadListener类。
agent_angle 无人车的朝向，2N*1的列向量。
u 控制器的输出，2N*1的列向量。
函数说明：
Timing_create() 中断函数，用于实现严格时间步长的控制计算，里面用于控制器的设计和计算，其他函数可以不动，将自己的公式放入其中用p和v信息计算出u即可。
OutPutByModel(float angle,Eigen::Matrix<float, 2, 1> u,geometry_msgs::Twist &vel_msg) 将控制器转换为线速度和角速度并输出，输入：小车朝向，控制器输入，速度话题；尽量不要动。
```

该代码实现的功能有：

- 相应的控制算法，让小车收敛到一致，应用的算法有NussFunction等。

**需要自己设计的函数：Timing_create()，里面进行控制器设计。**

2） 编译C++代码

- 添加编译代码

文件：`/home/ubuntu/catkin_ws/src/mrs-tb3/demo/tb3_gazebo/CMakeLists.txt`

```bash
gedit /home/ubuntu/catkin_ws/src/mrs-tb3/demo/tb3_gazebo/CMakeLists.txt
```

在Build后面添加如下代码：

```cmake
add_executable(code_name src/code_name.cpp)
target_link_libraries(code_name ${catkin_LIBRARIES})
```

其中code_name是代码的名字，比如编译代码`tb3_gazebo_luo_2024.cpp`：

```cmake
add_executable(tb3_gazebo_luo_2024 src/tb3_gazebo_luo_2024.cpp)
target_link_libraries(tb3_gazebo_luo_2024 ${catkin_LIBRARIES})
```

- 执行编译指令

```bash
cd ~/catkin_ws && catkin_make
```

3）运行仿真，进入gazebo，以`tb3_gazebo_luo_2024.cpp`为例

```
roslaunch tb3_gazebo multi_turtlebot3_rigidity.launch
```

4）运行自己的代码

```
rosrun tb3_gazebo tb3_gazebo_luo_2024
```

# 3 编队形成与变换

### 3.1 三角形编队

六个机器人使用PID控制器完成三角形编队，队形形成后停留固定时间，之后跟随虚拟领导者进行有规律的运动。

1）启动仿真。使用默认的空白地图环境中加载6个Turlebot3机器人，初始位置数据在triangles.cpp文件的注释里。

```
roslaunch learning_sanjiaoxing multi_turtlebot3-wyf.launch
```

2）启动.cpp文件，完成编队。

```
rosrun learning_sanjiaoxing triangles
```

### 3.2 编队队形变换

六个机器人首先由一字型编队变为三角形编队，队形形成后停留固定时间，再由三角形编队变为六边形编队，同样队形形成后停留固定时间，最后由六边形编队变为长方形编队，整个队形变换完成。

1）启动仿真。使用默认的空白地图环境中加载6个Turlebot3机器人。

```
roslaunch learning_sanjiaoxing multi_turtlebot3-wyf.launch
```

2）启动.cpp文件，编队变换完成。

```
rosrun learning_sanjiaoxing evolution
```

### 3.3 基于匈牙利算法和ICP算法的编队控制
15个机器人分布在四周，然后按照W A N G字样变换队形，期间可以通过键盘操控，改变某个机器人位置，可以观察到编队形成过程中的分配发生改变。由于ICP的作用队形也发生了旋转平移

1）启动仿真

```
roslaunch formation_control multi_turtlebot3-lzg.launch
```

2）启动.cpp文件,算法开始运行。
```
rosrun formation_control test_Algorithm_15
```

2）启动键盘节点。

```
rosrun turtlebot3_teleop turtlebot3_teleop_key
```

# 4 实验教程

设备：工控机

环境：ubuntu16.04

代码路径：/home/ubuntu/catkin_ws/src/mrs-tb3/demo/tb3_base_camera_control/src

## 4.1 编写C++代码

参考代码mrs-tb3/demo/tb3_base_camera_control/src/tb3_base_camera_control_DHLuo_MASs_leaderless.cpp

```shell
变量说明：
Robot_number 无人车的数量，根据实验设定，该代码为4。
N 无人车的数量，根据实验设定，该代码为4。
p 无人车的位置信息，2N*1的列向量。
v 无人车的速度信息，2N*1的列向量，该代码注释掉了速度的获取，详见multiThreadListener类。
函数说明：
Timing_create() 中断函数，用于实现严格时间步长的控制计算，里面用于控制器的设计和计算，其他函数可以不动，将自己的公式放入其中用p和v信息计算出u即可。
OutPutByModel(float angle,Eigen::Matrix<float, 2, 1> u,geometry_msgs::Twist &vel_msg) 将控制器转换为线速度和角速度并输出，输入：小车朝向，控制器输入，速度话题；尽量不要动。
APF(Eigen::Matrix<double, 2, 1> robotpoint,Eigen::Matrix<double, 2, 1>  barrierpoint) 人工势场法函数，输入：无人车A的位置信息，和无人车B的位置信息。
```

该代码实现的功能有：

1.APF人工势场法避碰；

2.通过键盘来让小车停止和运动，按键S：停，按键W：运动；

3.相应的控制算法，例如NussFunction等。

**需要自己设计的函数：Timing_create()，里面进行控制器设计。**

## 4.2 编译C++代码

### 4.2.1 添加编译代码

文件：/home/ubuntu/catkin_ws/src/mrs-tb3/demo/tb3_base_camera_control/CMakeLists.txt

```bash
gedit /home/ubuntu/catkin_ws/src/mrs-tb3/demo/tb3_base_camera_control/CMakeLists.txt
```

在Build后面添加如下代码：

```cmake
add_executable(code_name src/code_name.cpp)
target_link_libraries(code_name ${catkin_LIBRARIES})
```

其中code_name是代码的名字，比如编译代码tb3_base_camera_control_DHLuo_MASs_leaderless.cpp：

```cmake
add_executable(tb3_base_camera_control_DHLuo_MASs_leaderless src/tb3_base_camera_control_DHLuo_MASs_leaderless.cpp)
target_link_libraries(tb3_base_camera_control_DHLuo_MASs_leaderless ${catkin_LIBRARIES})
```

### 4.2.2 编译代码

在~/catkin_ws目录下

```bash
cd ~/catkin_ws
catkin_make -j8 -l8 
```

## 4.3 实验步骤及运行程序

### 4.3.1 启摄像头和二维码检测算法

1)清除所有节点

```
 ./multi_scripts/multi_kill.sh
```

2)运行摄像头

```shell
roslaunch mindvision_cam ge300gc.launch
```

3)二维码节点

```
roslaunch apriltag_ros continuous_detection.launch 
```

### 4.3.2 启动机器人及运行程序

1)在根目录下执行脚本，根据实验机器人数量启动相应代码，注意机器人的顺序编号。

```shell
#工控机器指令
cd ~ && ./multi_scripts/multi_robot.sh   #启动三个机器人

cd ~ && ./multi_scripts/multi_robot4.sh  #启动四个机器人

cd ~ && ./multi_scripts/multi_robot5.sh  #启动五个机器人

cd ~ && ./multi_scripts/multi_robot6.sh  #启动六个机器人

cd ~ && ./multi_scripts/multi_robot10.sh #启动十个机器人
#启动单个机器人
cd ~ && ./multi_scripts/single_robot.sh 0 #启动第0个机器人

#服务器指令
cd ~ && ./multi_scripts/multi_robot.sh 3 #启动三个机器人
cd ~ && ./multi_scripts/multi_robot.sh 4 #启动四个机器人
cd ~ && ./multi_scripts/multi_robot.sh 5 #启动五个机器人

```

2)运行自己的代码，路径无所谓

```shell
rosrun tb3_base_camera_control code_name #运行刚刚编译通过的程序，code_name是代码的名字
```

比如：

```shell
rosrun tb3_base_camera_control tb3_base_camera_control_DHLuo_MASs_leaderless #运行程序tb3_base_camera_control_DHLuo_MASs_leaderless.cpp
```

3)用键盘控制turtlebot3

```shell
// 启动机器人的键盘控制程序
./multi_scripts/multi_key.sh 0
./multi_scripts/multi_key.sh 1
./multi_scripts/multi_key.sh 2

./multi_scripts/multi_key.sh 3
./multi_scripts/multi_key.sh 4
./multi_scripts/multi_key.sh 5

./multi_scripts/multi_key.sh 6
./multi_scripts/multi_key.sh 7
./multi_scripts/multi_key.sh 8
./multi_scripts/multi_key.sh 9
```

### 4.3.3 发布路径话题

该话题以及编译过，直接运行即可，不要将没用到的机器人放入摄像头视野范围内。

```
roslaunch tb3_base_camera_control tb3_base_camera_paths.launch 
```

### 4.3.4 录制数据

```
rosbag record /tb3_0/robot /tb3_1/robot /tb3_2/robot /tb3_3/robot  /tb3_4/robot -o ~/catkin_ws/src/mrs-tb3/demo/tb3_base_camera_control/rosbag/robot.bag
```

其中/tb3_0/robot是话题名称，尽量不要更改，如何发现录制失败，可以看看是不是话题名称被改了，由C++代码里设定：

```C++
tb3_0_robot = node.advertise<nav_msgs::Odometry>("/tb3_0/robot", 10);
```

# 5 代码上传

```bash
cd ~/catkin_ws/src/mrs-tb3
git log #查看以往的日志
git add . #"."是添加所有文件， 也可以添加指定文件，例如git add README.md
git commit -m "周政：增加实验文档说明" #规范添加注释，必须有！！！
git push #上传代码，后面输入自己的gitee的帐号与密码
```

# 6 BUG

## 6.1 CV版本不兼容

摄像头代码会调用CV，会因为版本不同，安装路径不一样，头文件不一样，需要修改两个文件：

`sensor_interface/mindvision_cam/src/stereo_cam.cpp`和`sensor_interface/mindvision_cam/src/stereo_cam_ge300gc.cpp`

```c++
//台式机robot
// #include <cv.h>
#include <opencv2/opencv.hpp> //需要注释掉
#include <opencv2/highgui.hpp>
#include "opencv2/core/core.hpp"
#include "opencv2/highgui/highgui.hpp"
// #include "cxcore.hpp" //需要注释掉
//工控机ubuntu
#include <cv.h>
#include <highgui.h>
#include "opencv2/core/core.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "cxcore.hpp"
```

## 6.2 更换新机器人密钥报错

```
ssh 192.168.168.116
```

出现如下报错:

```
ubuntu@T3MULTIC-V110-98:~$ ssh 192.168.168.116
@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
@    WARNING: REMOTE HOST IDENTIFICATION HAS CHANGED!     @
@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
IT IS POSSIBLE THAT SOMEONE IS DOING SOMETHING NASTY!
Someone could be eavesdropping on you right now (man-in-the-middle attack)!
It is also possible that a host key has just been changed.
The fingerprint for the ECDSA key sent by the remote host is
SHA256:off60hI+cab9zTgdZkIHoy0CG5UROW1rdB0ib2whTB0.
Please contact your system administrator.
Add correct host key in /home/ubuntu/.ssh/known_hosts to get rid of this message.
Offending ECDSA key in /home/ubuntu/.ssh/known_hosts:12
  remove with:
  ssh-keygen -f "/home/ubuntu/.ssh/known_hosts" -R 192.168.168.116
ECDSA host key for 192.168.168.116 has changed and you have requested strict checking.
Host key verification failed.
```

解决办法:

```
ssh-keygen -f "/home/ubuntu/.ssh/known_hosts" -R 192.168.168.116
```

## 6.3 更换新机器人远程连接:

远程电脑ssh连接后:

```
sudo apt install tightvncserver
vncserver :1 -geometry 1920x1080 -depth 24
#注意： 密码统一设置为ubuntu
```