老规矩,先看效果!

前几天基于Python + OpenCV的ROI感兴趣目标提取做了单目标锁定追踪的功能包,订阅usb_cam发布出的图像话题,通过cv_bridge实现ROS图像话题和Python OpenCV图像格式的转换工作来实现功能。

这里我又给做出了增加功能——输出当前被追踪目标在当前图像下的六个数值,分别是被追踪目标坐标的X轴数值、被追踪目标坐标的Y轴数值、被追踪目标的Weight宽度数值、被追踪目标的Height高度数值、被追踪目标的X轴中心坐标值、被追踪目标的X轴中心坐标值。

我在这里的思路是这样的,目标追踪节点在检测到目标后需要画框,这个矿是通过x、y、w、h进行描述的。这里我另外增加的目标的中心坐标点数值,这个数值会用在我们接下来的PID云台上面,内容回放到后面再进行更新。

所以我需要在object_tracker功能包里面增加一个自定义的消息文件,因为这个消息是用来描述被追踪目标位置的,所以我命名为object_location.msg

我当前的文件系统是这样的。

文件组成方面,增加了一个object_location.msg的文件,这个是用来发布的话题类型。

然后就算scripts下的object_tracker.py改成了object_tracker_demo.py,因为之前的object_tracker和功能包名重复,这里为什么修改一下留给大家思考。

最后的话就是在CMakeList.txt和package.xml部位的修改。

object_location.msg的文件内容,六个数据都是int64类型的数据。

我们可以通过rosmsg命令来看一下。

rosmsg show object_tracker/object_location

然后是我现在的CMakeList.txt文件,内容如下。

主要是add_meeeage_file和generate_message这两个位置增加了自定义的内容。

我们在add_meeeage_file里面添加的是我们定义的消息文件,而在generate_message里面添加的则是消息类型文件(姑且先这样称呼,我的理解)。

消息类型文件是什么意思呢?std_msgs的String、sensor_msgs的JointState等,大家可以自己体会一下。

我们这里呢,这个就是object_tarcker的object_location,这个地方需要你们自己转过来,加油!

然后来瞅瞅我们package.xml文件的内容,如下。

这里的变化不是很大,增加了message_generation的编译依赖项(build_depend)和message_runtime的运行依赖项(exec_depend)。

最后就是我修改后的object_tracker_demo.py的内容,它是这样的。

#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import rospy	#导入rospy
import cv2		#导入cv2
from cv_bridge import CvBridge, CvBridgeError	#导入cv_bridge
from sensor_msgs.msg import Image	#导入sensor_msgs
from object_tracker.msg import object_location

import argparse	#导入argparse

ap = argparse.ArgumentParser()	#读取后缀参数
ap.add_argument("-t", "--tracker", type=str, default="kcf", help="OpenCV object tracker type")	#后缀参数t用于指定模型
args = vars(ap.parse_args())	#获取参赛

(major, minor) = cv2.__version__.split(".")[:2]	#检测当前OpenCV版本

#版本小于等于3的和大于3的部分参数不一样
if int(major) == 3 and int(minor) < 3:
	tracker = cv2.Tracker_create(args["tracker"].upper())
else:
	OPENCV_OBJECT_TRACKERS = {
		"csrt": cv2.TrackerCSRT_create,					#CSRT求解器
		"kcf": cv2.TrackerKCF_create,					#KCF求解器
		#"boosting": cv2.TrackerBoosting_create,		#BOOSTING求解器,已弃用
		"mil": cv2.TrackerMIL_create,					#MIL求解器
		#"tld": cv2.TrackerTLD_create,					#TLD求解器,已弃用
		#"medianflow": cv2.TrackerMedianFlow_create,	#MedianFlow求解器,已弃用
		#"mosse": cv2.TrackerMOSSE_create				#MOSSE求解器,已弃用
	}
	tracker = OPENCV_OBJECT_TRACKERS[args["tracker"]]()	#读取参数加载模型
   
initBB = None	#初始化比对数据空间(ROI数据空间)

class image_converter:
	def __init__(self):    
		# 创建cv_bridge,声明图像的发布者和订阅者
		self.image_pub = rospy.Publisher("cv_bridge_image", Image, queue_size=1)
		self.bridge = CvBridge()
		self.image_sub = rospy.Subscriber("/usb_cam/image_raw", Image, self.callback)
		
		self.object_location_pub = rospy.Publisher("object_location",object_location,queue_size=1)
		self.object_location = object_location()

	def callback(self,data):
		# 使用cv_bridge将ROS的图像数据转换成OpenCV的图像格式
		try:
			cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
		except CvBridgeError as e:
			print(str(e))

		(H, W) = cv_image.shape[:2]	#获取图像的Height和Weight
		
		global initBB	
		
		#如果ROI数据不为空,说明有需要追踪的目标
		if initBB is not None:
			(success, box) = tracker.update(cv_image)	#将ROI数据和当前帧进行比对,返回比对结果和ROI区域值
			
			#如果比对结果为真,说明在当前帧图像内检测到需要追踪的目标
			if success:
				(x, y, w, h) = [int(v) for v in box]	#从ROI区域值提取x、y、w、h四个数值
				cv2.rectangle(cv_image, (x, y), (x + w, y + h), (0, 255, 0), 2)	#画框
				
				object_location.x = int(x)
				object_location.y = int(y)
				object_location.w = int(w)
				object_location.h = int(h)
				object_location.x_center = int(x+w/2)
				object_location.y_center = int(y+h/2)
				self.object_location_pub.publish(self.object_location)
			
			#制作提示信息	
			info = [
				("Tracker", args["tracker"]),
				("Success", "Yes" if success else "No"),
			]
			
			#将提示信息显示在图像上面
			for (i, (k, v)) in enumerate(info):
				text = "{}: {}".format(k, v)
				cv2.putText(cv_image, text, (10, H - ((i * 20) + 20)), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 0, 255), 2)
				
		cv2.imshow("cv_image", cv_image)	#使用OpenCV进行显示
			
		key = cv2.waitKey(1) & 0xFF	#做个按键检测
		#如果按下了's',通过鼠标选择要追踪的目标
		if key == ord("s"):
			initBB = cv2.selectROI("cv_image", cv_image, fromCenter=False, showCrosshair=True)
			tracker.init(cv_image, initBB)	#将选中的内容传入到initBB

		# 再将opencv格式额数据转换成ros image格式的数据发布
		try:
			self.image_pub.publish(self.bridge.cv2_to_imgmsg(cv_image, "bgr8"))
		except CvBridgeError as e:
			print(str(e))

if __name__ == '__main__':
	try:
		#初始化ros节点
		rospy.init_node("object_tracker")
		rospy.loginfo("object_tracking...")
		image_converter()
		rospy.spin()
	except KeyboardInterrupt:
		print("Shutting down object_tracker node.")
		cv2.destroyAllWindows()

不同的地方,首先:

from object_tracker.msg import object_location

这里没啥好解释的,直接跳过!紧接着是:

self.object_location_pub = rospy.Publisher("object_location",object_location,queue_size=1)
self.object_location = object_location()

这里我是直接在古月老师定义的类里面写的,先是声明了一个发布,发布的话题名是object_location,发布的话题类型是object_location。

第二行就是声明了一个object_location类型的变量。最后就是获取数据发出去:

object_location.x = int(x)
object_location.y = int(y)
object_location.w = int(w)
object_location.h = int(h)
object_location.x_center = int(x+w/2)
object_location.y_center = int(y+h/2)

self.object_location_pub.publish(self.object_location)

在获取了所有的object_location的数据之后,我们就需要通过publish将这个内容发布出去即可。

唔,最后的话就是这个样子了的。

最后吧,我还是说一句话~

ROS = 通讯机制+工具箱+功能包+社区

ROS只是一个工具,它不是核心。你的思想、你的代码才是项目的核心,一定一定一定不要本末倒置。

初入门者,切记!