0x00 官方通知
最新消息:OriginBot V2.0.1版本正式发布,新增二维码识别与跟踪,点击查看
0x01 qr_code_detection 二维码识别源码解读
SSH连接OriginBot成功后,在终端中输入如下指令,启动二维码识别功能:
ros2 launch qr_code_detection qr_code_detection.launch.py
我们来看下 qr_code_detection.launch.py 文件里面写了什么:
originbot / originbot_example / qr_code_detection / src / qr_code_detection / launch / qr_code_detection.launch.py 文件启动了如下节点:
- web_service_launch_include,
- mipi_cam_node,
- image_transport_node,
- qr_detection_node,
- hobot_codec_node,
- websocket_node,
首先启动MIPI摄像头,发布640x480的nv12图像;
再使用旭日X3pi 开发平台的图像转换工具 hobot_codec,订阅 /qrcode_detected/img_result,将ros图像转为网页可显示的jpeg图像,供 websocket_node 显示给用户;
重点是 qr_detection_node 这个功能节点,代码如下:
# Copyright (c) 2022, www.guyuehome.com
# 导入库
import rclpy
import cv2
import cv_bridge
from rclpy.node import Node
from sensor_msgs.msg import CompressedImage
from sensor_msgs.msg import Image
from std_msgs.msg import String
from geometry_msgs.msg import Pose
import numpy as np
# 模型文件
modelPath = "/userdata/dev_ws/src/originbot/originbot_example/qr_code_detection/model/"
#预定义变量, 用于画框;
# TODO(ning 2023.10.29): 可以使用旭日X3pi的 'smart_topic' 功能;
color = (0, 0, 255)
thick = 3
font_scale = 0.5
font_thickness = 2
class QrCodeDetection(Node):
def __init__(self):
super().__init__('qrcode_detect')
self.bridge = cv_bridge.CvBridge()
# 接受来自utils/NV122BGR的imgae_out
self.image_sub = self.create_subscription(
CompressedImage, "/image_out/compressed", self.image_callback, 10)
# 发布绘制后的处理结果
self.pub_img = self.create_publisher(
Image, '/qrcode_detected/img_result', 10)
# 接收二维码信息
self.pub_qrcode_info = self.create_publisher(
String, "/qrcode_detected/info_result", 10)
self.info_result = String()
# 发布二维码的Pose
self.pub_qrcode_pose = self.create_publisher(
Pose, "/qrcode_detected/pose_result", 1)
self.pose_result = Pose()
# 重点: 使用 微信的qrcode 识别功能;
self.detect_obj = cv2.wechat_qrcode_WeChatQRCode(
modelPath+'detect.prototxt', modelPath+'detect.caffemodel',
modelPath+'sr.prototxt', modelPath+'sr.caffemodel')
def image_callback(self, msg):
cv_image = self.bridge.compressed_imgmsg_to_cv2(msg)
qrInfo, qrPoints = self.detect_obj.detectAndDecode(cv_image)
emptyList = ()
if qrInfo != emptyList:
self.get_logger().info('qrInfo: "{0}"'.format(qrInfo))
self.get_logger().info('qrPoints: "{0}"'.format(qrPoints))
qrInfo_str = qrInfo[0]
self.info_result.data = qrInfo_str
self.pub_qrcode_info.publish(self.info_result)
# 获取qrPoints四个点坐标
points = qrPoints[0]
points_array = np.array(points, dtype=np.float32)
# 计算四边形的中心点
M = cv2.moments(points_array)
cX = int(M["m10"] / M["m00"])
cY = int(M["m01"] / M["m00"])
self.get_logger().info('center:({},{})'.format(cX, cY))
# 计算四边形的面积
area = cv2.contourArea(points_array)
self.get_logger().info('area:{}'.format(area))
self.pose_result.position.x = float(cX)
self.pose_result.position.y = float(cY)
self.pose_result.position.z = float(area)
self.pub_qrcode_pose.publish(self.pose_result)
for pos in qrPoints:
for p in [(0, 1), (1, 2), (2, 3), (3, 0)]:
start = int(pos[p[0]][0]), int(pos[p[0]][1])
end = int(pos[p[1]][0]), int(pos[p[1]][1])
cv2.line(cv_image, start, end, color, thick)
font = cv2.FONT_HERSHEY_SIMPLEX
text_position = (int(pos[0][0]), int(pos[0][1]) - 10)
cv2.putText(cv_image, qrInfo_str,
text_position, font, font_scale, color, font_thickness)
cv2.circle(cv_image, (cX, cY), 3, color, -1)
self.pub_img.publish(self.bridge.cv2_to_imgmsg(cv_image, 'bgr8'))
def main(args=None):
rclpy.init(args=args)
qrCodeDetection = QrCodeDetection()
while rclpy.ok():
rclpy.spin(qrCodeDetection)
qrCodeDetection.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
- 使用OpenCV微信wechat的二维码识别功能,就两三行代码实现了扫码功能;
- cv2.wechat_qrcode_WeChatQRCode() 加载模型文件;
- detect_obj.detectAndDecode() 识别图像中的二维码;
- 返回的结果包含了2个参数,第一个为识别到的二维码信息,第2个为二维码的位置点,二者都是list类型。
- 随后对返回的数据进行处理,封装为geometry_msgs.msg.Pose类型,其中 p.z 为二维码的面积;
三个重要的话题名称:
'/qrcode_detected/img_result' # 识别后的图像
"/qrcode_detected/info_result" # 识别出的文字信息
"/qrcode_detected/pose_result" # 识别出的位置信息 ros类型
运行成功之后,就可以在同一网络输入ros小车的ip地址查看图像数据了,
由于我在外地出差需要三个月的时间,这里就不贴图片啦。这些操作都已经很熟悉了, 参考我往期博文;
0x02 qr_code_control_node.py 二维码控制源码解读
有了二维码识别功能发布的话题之后,我们来控制小车
启动机器人底盘:
ros2 launch originbot_bringup originbot.launch.py
启动二维码控制节点发布速度话题:
ros2 run qr_code_control qr_code_control_node
二维码控制,识别二维码信息进行前、后、左、右控制;
可以使用微信扫描二维码,分别到以下信息:
Front:前进
Back:后退
Left:向左旋转
Right:向右旋转
代码在这里我就不贴了,挑重点讲一讲:
代码订阅 "/qrcode_detected/info_result" 和 "/qrcode_detected/pose_result" 二维码信息和位姿话题,根据识别到的信息发布 'cmd_vel' 速度话题控制小车;
0x02.1 控制模式
- (默认) 识别二维码内容 Front,、Back、 Left、 Right 通过字符串对小车控制;
- 识别二维码的相对位置实现对二维码的跟踪;
二维码字符串控制对应代码如下:
def setTwistWithQrInfo(self, qrcode_info: String):
info = qrcode_info.data
if 'Front' in info:
self.setTwist(0.1, 0.0)
elif 'Back' in info:
self.setTwist(-0.1, 0.0)
elif 'Left' in info:
self.setTwist(0.0, 0.4)
elif 'Right' in info:
self.setTwist(0.0, -0.4)
else:
self.setTwist(0.0, 0.0)
self.pubControlCommand()
代码解析:很简单,根据二维码内容前后移动,当二维码消失后停止移动;
二维码位置控制对应代码如下:
def setTwistWithQrPose(self, qrcode_pose: Pose):
pose = qrcode_pose
qrcode_x = pose.position.x
qrcode_size = pose.position.z
self.setTwistWithX(qrcode_x)
self.setTwistWithZ(qrcode_size)
self.pubControlCommand()
def setTwistWithX(self, qrcode_x):
if qrcode_x > X_CENTER_MIN and qrcode_x < X_CENTER_MAX:
self.setTwist(self.twist.linear.x, 0.0)
elif qrcode_x < X_CENTER_MIN or qrcode_x > X_CENTER_MAX:
self.setTwist(self.twist.linear.x, (1.0 - qrcode_x/X_CENTER_AVERAGE))
else:
self.get_logger().info("No X, cannot control!")
def setTwistWithZ(self, qrcode_size):
if qrcode_size >= Z_SIZE_MIN and qrcode_size <= Z_SIZE_MAX:
self.setTwist(0.0, self.twist.angular.z)
elif qrcode_size < Z_SIZE_MIN or qrcode_size > Z_SIZE_MAX:
self.setTwist((1.0 - qrcode_size/Z_SIZE_AVERAGE) * 0.8, self.twist.angular.z)
else:
self.get_logger().info("No Z, cannot control!")
x是二维码中心点坐标在x轴上的位置,二维码在小车左边时小车向左旋转,在小车右边时小车向右旋转;
z是二维码的面积,面积在阈值范围内,设置线速度x为0,角速度保持不变,超出阈值根据超出的比例设置线速度x,角速度保持不变;
知识点:
使用 cv2.moments() 函数可以计算图像轮廓的矩,计算中心点;
self.setTwist(self.twist.linear.x, (1.0 - qrcode_x/X_CENTER_AVERAGE)) # P 比例追踪器;
评论(1)
您还未登录,请登录后发表或查看评论