ROS2+智能导航小车 2024年7月12日 · 69 分钟
项目一：基于yolov5的目标检测
在 ROS2 上打通树莓派相机与 PC，结合 OpenCV 与 YOLOv5 实现小车摄像头画面的实时目标检测
1. 本节最终实现效果
PC通过ssh连接到树莓派，通过ros2的键盘节点控制小车移动，然后在树莓派上发布摄像头节点的数据，PC通过ros2的通信机制接收到这个图像信息，并通过opencv和yolov5对图像数据进行处理并检测，最终将拍摄到的画面和检测到的目标显示出来，实现小车移动目标检测功能。
本节一共需要开启4个终端，第一个是开启与esp32的通信，第二个是小车的移动键盘控制节点，第三个是远程树莓派相机数据发布节点，第四个是用来目标检测的。
后续所有的项目：小车移动控制节点必须运行在树莓派端，否则两个设备之间数据传输会出现问题，而目标检测，建图等操作可以用PC来接收ROS话题来处理数据。
==用到的命令汇总：==
# 第一个是开启与esp32的通信
ros2 run micro_ros_agent micro_ros_agent udp4 --port 8888 -v6
# 第二个是小车的移动键盘控制节点
ros2 run teleop_twist_keyboard teleop_twist_keyboard
# 第三个是远程树莓派相机数据发布节点
ros2 run image_tools cam2image --ros-args -p width:=640 -p height:=480 -p frequency:=30.0 -p device_id:=0
# 第四个是用来目标检测的
ros2 run ros2_yolov5 yolo_detect_2d --ros-args -p device:=cpu -p image_topic:=/image -p show_result:=True -p pub_result_img:=True运行效果如下：
下面我们开始详细说明
2. 安装依赖
首先，确保您的PC已经更新了系统并且安装了必要的依赖。以下是一些安装步骤，其中$ROS_DISTRO 是您的ROS2发行版（例如：foxy、galactic、我使用的是humble）：
sudo apt update
sudo apt install python3-pip ros-$ROS_DISTRO-vision-msgs
pip3 install -i https://pypi.tuna.tsinghua.edu.cn/simple yolov5 如果上面的运行报错 则运行下面这个
pip install yolov5 -i https://pypi.tuna.tsinghua.edu.cn/simple --break-system-packages3. 编写代码
3.1 创建一个包
进入 ros2_car_ws/src 目录，并运行包创建命令：
ros2 pkg create --build-type ament_python ros2_yolov5你的终端将返回一条消息，验证已创建名为 ros2_yolov5 的软件包及其所有必要的文件和文件夹。
软件包中包含下面的内容
3.2 编写代码
进入 ros2_car_ws/src/ros2_yolov5/ros2_yolov5 目录。请记住，该目录是一个与嵌套的ROS 2软件包同名的Python包。
创建一个名为yolov5_detect_2d.py的文件，复制下面的内容
from math import frexp
from traceback import print_tb
from torch import imag
from yolov5 import YOLOv5
import rclpy
from rclpy.node import Node
from ament_index_python.packages import get_package_share_directory
from rcl_interfaces.msg import ParameterDescriptor
from vision_msgs.msg import Detection2DArray, ObjectHypothesisWithPose, Detection2D
from sensor_msgs.msg import Image, CameraInfo # ROS 2 消息类型
from cv_bridge import CvBridge # ROS的OpenCV桥接
import cv2
import yaml # 用于图像处理的 OpenCV 和用于配置文件处理的 YAML
from ros2_yolov5.cv_tool import px2xy
import os
# Get the ROS distribution version and set the shared directory for YoloV5 configuration files.
ros_distribution = os.environ.get("ROS_DISTRO")
package_share_directory = get_package_share_directory('ros2_yolov5')
# Create a ROS 2 Node class YoloV5Ros2.
class YoloV5Ros2(Node):
def __init__(self):
super().__init__('ros2_yolov5')
self.get_logger().info(f"Current ROS 2 distribution: {ros_distribution}")
# Declare ROS parameters.
self.declare_parameter("device", "cuda", ParameterDescriptor(
name="device", description="Compute device selection, default: cpu, options: cuda:0"))
self.declare_parameter("model", "yolov5s", ParameterDescriptor(
name="model", description="Default model selection: yolov5s"))
self.declare_parameter("image_topic", "/image_raw", ParameterDescriptor(
name="image_topic", description="Image topic, default: /image_raw"))
self.declare_parameter("camera_info_topic", "/camera/camera_info", ParameterDescriptor(
name="camera_info_topic", description="Camera information topic, default: /camera/camera_info"))
# Read parameters from the camera_info topic if available, otherwise, use the file-defined parameters.
self.declare_parameter("camera_info_file", f"{package_share_directory}/config/camera_info.yaml", ParameterDescriptor(
name="camera_info", description=f"Camera information file path, default: {package_share_directory}/config/camera_info.yaml"))
# Default to displaying detection results.
self.declare_parameter("show_result", False, ParameterDescriptor(
name="show_result", description="Whether to display detection results, default: False"))
# Default to publishing detection result images.
self.declare_parameter("pub_result_img", False, ParameterDescriptor(
name="pub_result_img", description="Whether to publish detection result images, default: False"))
# 1. Load the model.
model_path = package_share_directory + "/config/" + self.get_parameter('model').value + ".pt"
device = self.get_parameter('device').value
self.yolov5 = YOLOv5(model_path=model_path, device=device)
# 2. Create publishers.
# 创建两个发布者：一个用于发布检测结果，另一个用于发布结果图像。
self.yolo_result_pub = self.create_publisher(
Detection2DArray, "yolo_result", 10)
self.result_msg = Detection2DArray()
self.result_img_pub = self.create_publisher(Image, "result_img", 10)
# 3. Create an image subscriber (subscribe to depth information for 3D cameras, load camera info for 2D cameras).
# 已设置图像主题和相机信息主题的订阅
image_topic = self.get_parameter('image_topic').value
self.image_sub = self.create_subscription(
Image, image_topic, self.image_callback, 10)
camera_info_topic = self.get_parameter('camera_info_topic').value
self.camera_info_sub = self.create_subscription(
CameraInfo, camera_info_topic, self.camera_info_callback, 1)
# Get camera information.
# 相机信息是从 YAML 文件中读取的
with open(self.get_parameter('camera_info_file').value) as f:
self.camera_info = yaml.full_load(f.read())
self.get_logger().info(f"default_camera_info: {self.camera_info['k']} \n {self.camera_info['d']}")
# 4. Image format conversion (using cv_bridge).
# 用于图像格式转换的 CVBridge 和用于显示/发布结果的参数
self.bridge = CvBridge()
self.show_result = self.get_parameter('show_result').value
self.pub_result_img = self.get_parameter('pub_result_img').value
# 当收到新的相机信息消息时，将调用此函数。它会更新相机参数，然后取消订阅该主题
def camera_info_callback(self, msg: CameraInfo):
"""
Get camera parameters through a callback function.
"""
self.camera_info['k'] = msg.k
self.camera_info['p'] = msg.p
self.camera_info['d'] = msg.d
self.camera_info['r'] = msg.r
self.camera_info['roi'] = msg.roi
self.camera_info_sub.destroy()
# 当收到新的图像消息时，将调用此函数。它处理图像，执行对象检测并发布结果
def image_callback(self, msg: Image):
# 5. Detect and publish results.
image = self.bridge.imgmsg_to_cv2(msg)
detect_result = self.yolov5.predict(image)
self.get_logger().info(str(detect_result))
self.result_msg.detections.clear()
self.result_msg.header.frame_id = "camera"
self.result_msg.header.stamp = self.get_clock().now().to_msg()
# Parse the results.
predictions = detect_result.pred[0]
boxes = predictions[:, :4] # x1, y1, x2, y2
scores = predictions[:, 4]
categories = predictions[:, 5]
for index in range(len(categories)):
name = detect_result.names[int(categories[index])]
detection2d = Detection2D()
detection2d.id = name
x1, y1, x2, y2 = boxes[index]
x1 = int(x1)
y1 = int(y1)
x2 = int(x2)
y2 = int(y2)
center_x = (x1+x2)/2.0
center_y = (y1+y2)/2.0
if ros_distribution=='galactic':
detection2d.bbox.center.x = center_x
detection2d.bbox.center.y = center_y
else:
detection2d.bbox.center.position.x = center_x
detection2d.bbox.center.position.y = center_y
detection2d.bbox.size_x = float(x2-x1)
detection2d.bbox.size_y = float(y2-y1)
obj_pose = ObjectHypothesisWithPose()
obj_pose.hypothesis.class_id = name
obj_pose.hypothesis.score = float(scores[index])
# px2xy
world_x, world_y = px2xy(
[center_x, center_y], self.camera_info["k"], self.camera_info["d"], 1)
obj_pose.pose.pose.position.x = world_x
obj_pose.pose.pose.position.y = world_y
detection2d.results.append(obj_pose)
self.result_msg.detections.append(detection2d)
# Draw results.
if self.show_result or self.pub_result_img:
cv2.rectangle(image, (x1, y1), (x2, y2), (0, 255, 0), 2)
cv2.putText(image, f"{name}({world_x:.2f},{world_y:.2f})", (x1, y1),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 1)
cv2.waitKey(1)
# Display results if needed.
if self.show_result:
cv2.imshow('result', image)
cv2.waitKey(1)
# Publish result images if needed.
if self.pub_result_img:
result_img_msg = self.bridge.cv2_to_imgmsg(image, encoding="bgr8")
result_img_msg.header = msg.header
self.result_img_pub.publish(result_img_msg)
if len(categories) > 0:
self.yolo_result_pub.publish(self.result_msg)
def main():
rclpy.init()
rclpy.spin(YoloV5Ros2())
rclpy.shutdown()
if __name__ == "__main__":
main()
创建一个名为cv_tool.py的文件，复制下面的的内容
# 导入所需的库
# Import the required libraries
import cv2 # OpenCV library for image processing
import numpy as np # NumPy library for array and matrix operations
# 相机内参矩阵K，包括相机的焦距和主点坐标
# Camera intrinsic matrix K, including camera's focal length and principal point coordinates
K = [[602.7175003324863, 0, 351.305582038406],
[0, 601.6330312976042, 240.0929104708551],
[0, 0, 1]]
# 相机畸变参数D，用于校正图像畸变
# Camera distortion parameters D, used for correcting image distortion
D = [0.06712174262966401, -0.2636999208734844,
0.006484443443073637, 0.01111161327049835, 0]
# 定义一个函数px2xy，用于将像素坐标转换为相机坐标系下的二维坐标
# Define a function px2xy to convert pixel coordinates to 2D coordinates in camera coordinate system
def px2xy(point, camera_k, camera_d, z=1.0):
# 将相机内参矩阵K和相机畸变参数D转换为NumPy数组
# Convert camera intrinsic matrix K and camera distortion parameters D to NumPy arrays
MK = np.array(camera_k, dtype=float).reshape(3, 3)
MD = np.array(camera_d, dtype=float)
# 将输入的像素坐标点转换为NumPy数组
# Convert the input pixel coordinate point to a NumPy array
point = np.array(point, dtype=float)
# 使用OpenCV的cv2.undistortPoints函数对输入点进行畸变矫正，并乘以深度值z
# Use OpenCV's cv2.undistortPoints function to correct distortion of input points and multiply by depth value z
pts_uv = cv2.undistortPoints(point, MK, MD) * z
# 返回相机坐标系下的二维坐标
# Return 2D coordinates in the camera coordinate system
return pts_uv[0][0]
# 调用函数并打印结果（如果需要）
# Call the function and print the result (if needed)
# print(px2xy([0, 0], K, D, 1))
3.3 参数配置
在package.xml中添加下面的内容
<depend>rclpy</depend>
<depend>vision_msgs</depend>
<depend>yolov5</depend>添加后的完整代码如下：
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>ros2_yolov5</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="lll@todo.todo">lll</maintainer>
<license>TODO: License declaration</license>
<depend>rclpy</depend>
<depend>vision_msgs</depend>
<depend>yolov5</depend>
<test_depend>ament_copyright</test_depend>
<test_depend>ament_flake8</test_depend>
<test_depend>ament_pep257</test_depend>
<test_depend>python3-pytest</test_depend>
<export>
<build_type>ament_python</build_type>
</export>
</package>在setup.py中添加
"yolo_detect_2d=ros2_yolov5.yolov5_detect_2d:main"如下：
from setuptools import find_packages, setup
from glob import glob
import os
package_name = 'ros2_yolov5'
setup(
name=package_name,
version='0.0.0',
packages=find_packages(exclude=['test']),
data_files=[
('share/ament_index/resource_index/packages',
['resource/' + package_name]),
('share/' + package_name, ['package.xml']),
(os.path.join('share', package_name, 'config'), glob('config/**')),
],
install_requires=['setuptools'],
zip_safe=True,
maintainer='lll',
maintainer_email='lll@todo.todo',
description='TODO: Package description',
license='TODO: License declaration',
tests_require=['pytest'],
entry_points={
'console_scripts': [
"yolo_detect_2d=ros2_yolov5.yolov5_detect_2d:main"
],
},
)
3.4 创建config文件夹
将yolov5s.pt模型文件放这里，可以随意换别的模型
创建一个相机参数文件camera_info.yaml，代码如下
height: 1000
width: 800
distortion_model: plumb_bob
d: [0.0, 0.0, 0.0, 0.0, 0.0]
k: [476.7030836014194, 0.0, 400.5, 0.0, 476.7030836014194, 400.5, 0.0, 0.0, 1.0]
r: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
p:
[
476.7030836014194,
0.0,
400.5,
-0.0,
0.0,
476.7030836014194,
400.5,
0.0,
0.0,
0.0,
1.0,
0.0,
]所有的代码以及完成，接下来就可以运行了。
4. 编译和运行
4.1 第一个终端：
ssh远程登录到树莓派后的终端
ssh user@192.168.1.100
# 然后输入密码即可登录启动摄像头节点发布消息
ros2 run image_tools cam2image --ros-args -p width:=640 -p height:=480 -p frequency:=30.0 -p device_id:=0最后的这里 device_id:=0 或者 device_id:=-1 都可以
此时大概率会出现如下的报错：
lll@laj:~/ros2_ws$ ros2 run image_tools cam2image --ros-args -p width:=640 -p height:=480 -p frequency:=30.0 -p device_id:=0
[ WARN:0@0.449] global ./modules/videoio/src/cap_gstreamer.cpp (2401) handleMessage OpenCV | GStreamer warning: Embedded video playback halted; module v4l2src0 reported: Could not open device '/dev/video0' for reading and writing.
[ WARN:0@0.449] global ./modules/videoio/src/cap_gstreamer.cpp (1356) open OpenCV | GStreamer warning: unable to start pipeline
[ WARN:0@0.449] global ./modules/videoio/src/cap_gstreamer.cpp (862) isPipelinePlaying OpenCV | GStreamer warning: GStreamer: pipeline have not been created
[ WARN:0@0.451] global ./modules/videoio/src/cap_v4l.cpp (902) open VIDEOIO(V4L2:/dev/video0): can't open camera by index
[ERROR] [1720852535.682295122] [cam2image]: Could not open video stream
terminate called after throwing an instance of 'std::runtime_error'
what(): Could not open video stream
[ros2run]: Aborted这个问题通常是因为通过 SSH 远程登录到树莓派时，无法直接访问连接在树莓派上的摄像头设备。摄像头设备通常需要物理访问权限，而通过 SSH 远程登录时，默认情况下不会授予这种权限。
只需要通过下面的命令确保当前用户对 /dev/video0 设备具有读写权限
sudo chmod 666 /dev/video0如下图所示，查看此时的话题，相机发布的话题为/image
ros2 topic list
此时摄像头节点会发布图像消息到ROS2的话题上，我们在PC端接通过订阅这个话题/image来接收摄像头发布的图像消息。
4.2 第二个终端：
首先要编译项目：
cd ros2_car_ws
colcon build ## colcon build --packages-select ros2_yolov5
source install/setup.bash现在，您可以运行Yolo_ROS2节点。默认情况下，它将使用CPU来进行检测，您可以根据需要更改这些参数：
ros2 run ros2_yolov5 yolo_detect_2d --ros-args -p device:=cpu -p image_topic:=/image此时显示，只有命令行输出,
我们这里取出其中一条
可以看到有检测到的目标框的大小和类别数据
此时我们继续查看话题，发现多了我们编写的两个话题
ros2 topic list
/result_img /yolo_result
后续我们只需要将其绘制出来就完成了。
4.3 绘制识别到的目标图像
命令如下：
ros2 run ros2_yolov5 yolo_detect_2d --ros-args -p device:=cpu -p image_topic:=/image -p show_result:=True -p pub_result_img:=True此时就可以看到类似的画面了
4.4 参数解释
Yolo_ROS2将检测结果发布到/yolo_result话题中，包括原始像素坐标以及归一化后的相机坐标系下的x和y坐标。您可以使用以下命令查看检测结果：
ros2 topic echo /yolo_result
4.4.1 参数设置
在运行Yolo_ROS2节点时，您可以使用 -p name:=value 的方式来修改参数值。
4.4.2 图像话题
您可以通过指定以下参数来更改图像话题：
image_topic:=/image4.4.3 计算设备设置
如果您有CUDA支持的显卡，可以选择以下参数来配置计算设备：
device:=cpu4.4.4 是否实时显示结果
您可以使用以下参数来控制是否实时显示检测结果。设置为True将实时显示结果，设置为False则不会显示：
show_result:=False请注意，实时显示中的cv2.imshow可能会卡住。如果只需要验证结果，可以将此参数设置为False。
4.4.5 切换不同Yolov5模型
默认情况下，Yolo_ROS2使用yolov5s模型。您可以通过以下参数来更改模型：
model:=yolov5m4.4.6 相机参数文件
功能包默认从 /camera/camera_info 话题获取相机参数，在获取成功前，相机参数文件路径可以通过参数进行设置，参数为：camera_info_file，通过该参数可以设置文件路径，注意需要使用绝对目录：
-p camera_info_file:=/home/lll/ros2_ws/src/ros2_yolov5/config/camera_info.yaml5. 控制小车移动
运行agent，点击RST按键。
sudo docker run -it --rm -v /dev:/dev -v /dev/shm:/dev/shm --privileged --net=host microros/micro-ros-agent:$ROS_DISTRO udp4 --port 8888 -v6因为esp32的wifi有时不太稳定，只有当出现下图右侧所示，才可以稳定控制小车，
看到连接建立表示通信成功，接着用ros2 topic list
ros2 topic list看到/cmd_vel表示正常，接着我们使用teleop_twist_keyboard进行键盘控制
ros2 run teleop_twist_keyboard teleop_twist_keyboard先调整下速度，降低到0.05左右（50cm/s），然后使用i\j\j\k,测试。