在GAZEBO中使用PX4+YOLOv5实现目标跟踪
任务流程
1、在ubuntu20.04环境下部署Yolov5算法；
2、创建目标检测节点，订阅图像信息、调用Yolov5进行识别、并发布识别结果；
3、创建目标跟踪节点，订阅识别结果、控制无人机进行目标跟踪。
4、关于px4速度控制坐标系的一些补充
1、部署Yolov5
本机的环境是ubuntu20.04和ros noetic，默认使用python3.8，为了使用方便，我们需要安装对应版本的Yolov5，否则在使用时需要频繁更换配置虚拟环境，对小白不太友好。
具体安装流程参考官方文档，相当的宝宝巴士：https://varhowto.com/install-pytorch-ubuntu-20-04/
Yolov5需要依赖GPU才能获得较好的性能，建议安装带GPU加速的版本，跟着官方教程安装完后，对Pytorch和Anaconda都会有一定程度的认识和了解。
2、目标识别
该部分内容参考了博客https://blog.csdn.net/cy1641395022/article/details/132237482
新建ros工作空间
mkdir -p ~/yolov5_ws/src
cd ~/yolov5_ws
catkin_make
创建新的ros package
然后进行编译
cd ~/yolov5_ws
catkin_make
source devel/setup.bash
创建scripts目录，并创建python脚本
cd ~/yolov5_ws/src/yolov5_ros
mkdir scripts
cd scripts
touch detect_ros.py
chmod +x detect_ros.py
随后粘贴以下代码到detect_ros.py文件中
#! /usr/bin/env python3
# -*- coding: utf-8 -*-
import rospy
import cv2
import torch
import numpy as np
import warnings
from functools import partial
from cv_bridge import CvBridge, CvBridgeError
from std_msgs.msg import Header
from geometry_msgs.msg import Twist
from sensor_msgs.msg import Image, RegionOfInterest
from yolov5_ros.msg import BoundingBox, BoundingBoxes
class Yolov5Param:
def __init__(self):
# load local repository(YoloV5:v6.0)
# 指定yolov5的源码路径，位于/home/rui27/yolov5/
yolov5_path = rospy.get_param('/yolov5_path', '')
# 指定yolov5的权重文件路径，位于/home/rui27/yolov5/yolov5s.pt
weight_path = rospy.get_param('~weight_path', '')
# yolov5的某个参数，这里不深究了
conf = rospy.get_param('~conf', '0.5')
# 使用pytorch加载yolov5模型，torch.hub.load会从/home/rui27/yolov5/中找名为hubconf.py的文件
# hubconf.py文件包含了模型的加载代码，负责指定加载哪个模型
self.model = torch.hub.load(yolov5_path, 'custom', path=weight_path, source='local')
# 一个参数，用来决定使用cpu还是gpu，这里我们使用gpu
if (rospy.get_param('/use_cpu', 'false')):
self.model.cpu()
else:
self.model.cuda()
self.model.conf = conf
# target publishers
# BoundingBoxes是本样例自定义的消息类型，用来记录识别到的目标
# 使用/yolov5/targets topic发布出去
self.target_pub = rospy.Publisher("/yolov5/targets", BoundingBoxes, queue_size=1)
def image_cb(msg, cv_bridge, yolov5_param, color_classes, image_pub):
# 使用cv_bridge将ROS的图像数据转换成OpenCV的图像格式
try:
# 将Opencv图像转换numpy数组形式，数据类型是uint8（0~255）
# numpy提供了大量的操作数组的函数，可以方便高效地进行图像处理
cv_image = cv_bridge.imgmsg_to_cv2(msg, "bgr8")
frame = np.array(cv_image, dtype=np.uint8)
except (CvBridgeError, e):
print(e)
# 实例化BoundingBoxes，存储本次识别到的所有目标信息
bounding_boxes = BoundingBoxes()
bounding_boxes.header = msg.header
# 将BGR图像转换为RGB图像, 给yolov5，其返回识别到的目标信息
rgb_image = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
results = yolov5_param.model(rgb_image)
boxs = results.pandas().xyxy[0].values
for box in boxs:
bounding_box = BoundingBox()
# 置信度，因为是基于统计，因此每个目标都有一个置信度，标识可能性
bounding_box.probability =np.float64(box[4])
# （xmin, ymin）是目标的左上角，（xmax,ymax）是目标的右上角
bounding_box.xmin = np.int64(box[0])
bounding_box.ymin = np.int64(box[1])
bounding_box.xmax = np.int64(box[2])
bounding_box.ymax = np.int64(box[3])
# 本地识别到的目标个数
bounding_box.num = np.int16(len(boxs))
# box[-1]是目标的类型名，比如person
bounding_box.Class = box[-1]
# 放入box队列中
bounding_boxes.bounding_boxes.append(bounding_box)
# 同一类目标，用同一个颜色的线条画框
if box[-1] in color_classes.keys():
color = color_classes[box[-1]]
else:
color = np.random.randint(0, 183, 3)
color_classes[box[-1]] = color
# 用框把目标圈出来
cv2.rectangle(cv_image, (int(box[0]), int(box[1])),
(int(box[2]), int(box[3])), (int(color[0]),int(color[1]), int(color[2])), 2)
# 在框上, 打印物体类型信息Class
if box[1] < 20:
text_pos_y = box[1] + 30
else:
text_pos_y = box[1] - 10
cv2.putText(cv_image, box[-1],
(int(box[0]), int(text_pos_y)-10), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 255), 2, cv2.LINE_AA)
# 发布目标数据，topic为：/yolov5/targets
# 可以使用命令查看：rotopic echo /yolov5/targets
yolov5_param.target_pub.publish(bounding_boxes)
# 将标识了识别目标的图像转换成ROS消息并发布
image_pub.publish(cv_bridge.cv2_to_imgmsg(cv_image, "bgr8"))
def main():
warnings.simplefilter("ignore", category=FutureWarning)
rospy.init_node("detect_ros")
rospy.loginfo("starting detect_ros node")
bridge = CvBridge()
yolov5_param = Yolov5Param()
color_classes = {}
image_pub = rospy.Publisher("/yolov5/detection_image", Image, queue_size=1)
bind_image_cb = partial(image_cb, cv_bridge=bridge, yolov5_param=yolov5_param, color_classes=color_classes, image_pub=image_pub)
rospy.Subscriber("/camera/color/image_raw", Image, bind_image_cb)
rospy.spin()
cv2.destroyAllWindows()
if __name__ == "__main__":
main()
该脚本创建了一个ros节点，订阅了相机发布的话题/camera/color/image_raw，随后调用yolov5进行识别，并把识别到的目标用在图像中框出，打上类别标签。
此时代码报错，因为在框选目标时用到了自定义的消息类型BoundingBox.msg和BoundingBoxes.msg，我们还需要在功能包中添加上该自定义的消息类型，并进行编译。
在yolov5_ros功能包中新建msg文件夹，并在文件夹中创建这两个消息
cd ~/yolov5_ws/src/yolov5_ros
mkdir msg
cd msg
touch BoundingBox.msg
touch BoundingBoxes.msg
定义BoundingBox.msg消息
float64 probability
int64 xmin
int64 ymin
int64 xmax
int64 ymax
int16 num
string Class
定义BoundingBoxes.msg消息
Header header
BoundingBox[] bounding_boxes
其中的BoundingBox.msg消息定义了一个检测框的信息，包括识别到的物体的长度、宽度、类别等信息。
而BoundingBoxes.msg这是在BoundingBox.msg的基础上创建了一个实例化的数组，用于框选一张图片中识别到的多个目标。
自定完消息后需要修改CMakeLists文件和package.xml文件，再重新编译。
CMakeLists需要添加的部分如下：
find_package(catkin REQUIRED COMPONENTS
cv_bridge
image_transport
roscpp
rospy
sensor_msgs
std_msgs
geometry_msgs
message_generation //用于生成自定义消息
)
//自定义消息文件
add_message_files(
FILES
BoundingBox.msg
BoundingBoxes.msg
)
//生成自定义消息所依赖的消息
generate_messages(
DEPENDENCIES
std_msgs
)
catkin_package(
CATKIN_DEPENDS message_runtime
)
catkin_install_python(PROGRAMS
scripts/detect_ros.py
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
package.xml需要添加的部分如下：
<build_depend>message_generation</build_depend>
<exec_depend>message_runtime</exec_depend>
修改完后能够成功编译代码
最后需要创建launch文件来启动该节点，在yolov5_ros功能包中新建launch文件夹，并在其中创建detect_ros.launch文件
mkdir launch
touch detect_ros.launch
在launch文件中复制如下代码：
<launch>
<param name="yolov5_path" value="/home/rui27/yolov5"/> //这里填写yolov5安装的路径
<param name="use_cpu" value="false" />
<!-- Start yolov5 and ros wrapper -->
<node pkg="yolov5_ros" name="detect_ros" type="detect_ros.py" output="screen" >
<param name="weight_path" value="/home/rui27/yolov5/yolov5s.pt"/> //这里填写yolov5训练权重文件的路径
<param name="image_topic" value="/camera/color/image_raw" /> //这里填写相机发布的ros话题名称
<param name="pub_topic" value="/yolov5/targets" />
<param name="conf" value="0.3" />
</node>
<!-- 使用launch-prefix让rqt_image_view比yolov5_detector晚七秒启动，因为yolov5启动比较慢 -->
<node
pkg="rqt_image_view"
type="rqt_image_view"
name="rqt_image_view"
launch-prefix="bash -c 'sleep 7; $0 $@'"
output="screen"
/>
</launch>
最后重新编译功能包，通过launch文件运行该节点
此外，添加上这些编译后缀可以生成供vscode识别的编译文件（vscode有时候报错或是无法进行函数跳转是因为无法认识工程，这样编译以后能解决此问题）
catkin_make -DCMAKE_BUILD_TYPE=Release -DCMAKE_EXPORT_COMPILE_COMMANDS=Yes
roslaunch yolov5_ros detect_ros.launch
启动该节点前需要先运行roscore，并且有对应的相机话题发布。
在终端中使用rostopic echo /yolov5/targets无法查看到该话题是因为该话题使用了我们自定义的ros消息，该消息只在该工作空间能被识别，所以需要先source该工作空间的setup.bash文件。
3、创建目标跟踪节点
新建target_track功能包
(因为我有一个专门负责无人机控制的工作空间uav_ws，所以我把该功能包创建在另外一个工作空间。一般情况下这个功能包可以也创建在yolov5_ws工作空间，这样比较方便用launch文件一起启动，而且也可以直接使用自定义的BoundingBox.msg消息。如果创建在了其他工作空间，也要在那个工作空间创建相同的自定义消息，否则在订阅与该消息相关的话题时会出错）
cd ~/uav_ws/src
catkin_create_pkg target_track roscpp std_msgs geometry_msgs mavros_msgs
cd target_track/src
gedit yolov5_track.cpp
复制如下代码到yolov5_track.cpp文件中
#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include<geometry_msgs/TwistStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include "target_track/BoundingBoxes.h"
ros::Publisher vel_pub;
int lostCount = 0;
bool captured = false;
int x, y;
float d;
float kp_yaw = 1.5;
float kp_x = 1.5;
float kp_z = 1;
geometry_msgs::TwistStamped target_vel;
target_track::BoundingBoxes boxes;
void target_cb(const target_track::BoundingBoxes::ConstPtr& msg){
// boxes = *msg;
for(const auto& box : msg->bounding_boxes){
if(box.Class == "person"){
captured = true;
lostCount = 0;
x = (box.xmin + box.xmax) / 2;
y = (box.ymin + box.ymax) / 2;
d = box.ymax - box.ymin;
ROS_INFO("Detected Target Person");
ROS_INFO("x: %d", x);
ROS_INFO("y: %d", y);
ROS_INFO("d: %f", d);
}
}
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "yolov5_track_node");
ros::NodeHandle nh;
ros::Rate rate(20);
ros::Subscriber target_sub = nh.subscribe<target_track::BoundingBoxes>("yolov5/targets", 10, target_cb);
ros::Publisher pos_pub = nh.advertise<geometry_msgs::PoseStamped>("mavros/setpoint_position/local", 10);
vel_pub = nh.advertise<geometry_msgs::TwistStamped>("mavros/setpoint_velocity/cmd_vel", 10);\
while(ros::ok()){
ROS_INFO("captured: %d", captured);
lostCount++;
if(lostCount > 1000) lostCount = 1000;
if(lostCount > 10) captured = false;
if(captured == false){
target_vel.twist.angular.z = 0;
target_vel.twist.linear.z = 0;
target_vel.twist.linear.x = 0;
}else if(captured == true){
target_vel.twist.angular.z = kp_yaw * (x - 320) * -0.005;
target_vel.twist.linear.z = kp_z * (y - 240) * -0.005;
target_vel.twist.linear.x = kp_x * (1.0 - d/240.0);
}
vel_pub.publish(target_vel);
ros::spinOnce();
rate.sleep();
}
}
在该工作空间创建自定义消息BoundingBox.msg和BoundingBoxes.msg，方法同前文。
接着修改CMakeLists.txt文件，在最后加上该节点的相关信息
add_executable(${PROJECT_NAME}_node src/yolov5_track.cpp)
target_link_libraries(${PROJECT_NAME}_node
${catkin_LIBRARIES}
完成后进行编译，并可以使用rosrun运行该节点（需要启动roscore，并且有前文detect_ros节点发布的yolov5/targets话题）
catkin_make
rosrun target_track target_track_node
4、修改mavros/setpoint_velocity/cmd_vel坐标系
完成上面内容就已经能够识别并控制px4运动，但是可能px4并不会按照设想方位运动，无法跟踪到目标。
这涉及到mavros消息坐标系的问题，mavros/setpoint_velocity/cmd_vel是用于对px4进行速度控制的话题，该话题默认使用全局坐标系，而我们发布的速度是无人机机体坐标系下的速度，下面进行修改。
在本机的环境中，我们运行PX4_Firmware/launch/mavros_posix_sitl.launch文件来进行px4无人机的仿真。
打开该文件，我们发现他调用了mavros/launch/px4.launch文件来启动仿真无人机，如下图所示。
因此我们需要先找到mavros功能包的位置
打开终端，输入以下指令：
rospack find mavros
终端显示mavros功能包的安装路径，如下图所示：
我们找到该路径下的px4.launch文件（/opt/ros/noetic/share/mavros/launch/px4.launch）
可以看到，启动该节点时的配置参数都从该目录下的px4_config.yaml文件中导入
于是我们去修改px4_config.yaml文件
图中的mav_frame参数用来设置该话题的坐标系
我们将他从LOCAL_END改为BODY_END
修改完成后保存即可，若无法保存的话，通过sudo nautilus指令以管理员权限打开文件管理器就有权限修改该文件了
修改完成后，再次运行之前的程序，无人机能够正确跟踪目标。
浙公网安备 33010602011771号