无人机自主物体跟随/循迹
- 1. 物体跟踪
-
- 1.1 实现思路
- 1.2 代码示例
- 2. 自主寻线
- 本实验采用ROS和OpenCV实现功能,实验平台采用Parrot的Bebop2无人机
- ROS部分的学习可以参考我的专栏:ROS学习记录
- 实验平台的操作方式见:ROS控制Parrot Bebop2无人机
1. 物体跟踪
1.1 实现思路
调用无人机的图像:
cv_image = self.bridge.imgmsg_to_cv2(data, “bgr8”)
之后同OpenCV实现机器人对物体进行移动跟随一样,获取所要跟踪的物体 节点的发布和接收见:ROS学习: Topic通讯
1.2 代码示例
import rospy
import cv2 as cv
from geometry_msgs.msg import Twist
from cv_bridge import CvBridge, CvBridgeError
from sensor_msgs.msg import Image
class image_converter:
def __init__(self):
self.cmd_pub = rospy.Publisher("/bebop/cmd_vel", Twist, queue_size=1) # 发布运动控制信息
self.bridge = CvBridge()
self.image_sub = rospy.Subscriber("/bebop/image_raw", Image, self.callback) # 订阅摄像头信息
def callback(self, data):
try:
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8") # 获取订阅的摄像头图像
except CvBridgeError as e:
print e
# 对图像进行处理
kernel = cv.getStructuringElement(cv.MORPH_RECT, (3, 3)) # 定义结构元素
height, width = cv_image.shape[0:2]
screen_center = width / 2
screen_center_h = height / 2
offset = 50
offset_h = 30
lower_b = (75, 43, 46)
upper_b = (110, 255, 255)
hsv_frame = cv.cvtColor(cv_image, cv.COLOR_BGR2HSV) # 转成HSV颜色空间
mask = cv.inRange(hsv_frame, lower_b, upper_b)
mask2 = cv.morphologyEx(mask, cv.MORPH_OPEN, kernel) # 开运算去噪
mask3 = cv.morphologyEx(mask2, cv.MORPH_CLOSE, kernel) # 闭运算去噪
cv.imshow("mask", mask3)
# 找出面积最大的区域
_, contours, _ = cv.findContours(mask3, cv.RETR_EXTERNAL, cv.CHAIN_APPROX_SIMPLE)
maxArea = 0
maxIndex = 0
for i, c in enumerate(contours):
area = cv.contourArea(c)
if area > maxArea:
maxArea = area
maxIndex = i
# 绘制轮廓
cv.drawContours(cv_image, contours, maxIndex, (255, 255, 0), 2)
# 获取外切矩形
x, y, w, h = cv.boundingRect(contours[maxIndex])
cv.rectangle(cv_image, (x, y), (x + w, y + h), (255, 0, 0), 2)
# 获取中心像素点
center_x = int(x + w / 2)
center_y = int(y + h / 2)
cv.circle(cv_image, (center_x, center_y), 5, (0, 0, 255), -1)
# 显示图像
cv.imshow("Image", cv_image)
# 运动控制
twist = Twist()
# 左右转向和移动
if center_x < screen_center - offset:
twist.linear.x = 0.0
twist.linear.y = 0.2
twist.angular.z = 0.2
print "turn left"
elif screen_center - offset <= center_x <= screen_center + offset:
twist.linear.x = 0.0
twist.linear.y = 0.0
twist.angular.z = 0
print "keep"
elif center_x > screen_center + offset:
twist.linear.x = 0.0
twist.linear.y = -0.2
twist.angular.z = -0.2
print "turn right"
else:
twist.linear.x = 0
twist.angular.z = 0
print "stop"
# 上下移动
if center_y < screen_center_h - offset_h:
twist.linear.z = 0.2
print "up up up"
elif screen_center_h - offset_h <= center_y <= screen_center_h + offset_h:
twist.linear.z = 0
print "keep"
elif center_y > screen_center_h + offset_h:
twist.linear.z = -0.2
print "down down down"
else:
twist.linear.z = 0
print "stop"
cv.waitKey(3)
# 发布运动指令
try:
self.cmd_pub.publish(twist)
except CvBridgeError as e:
print e
if __name__ == '__main__':
try:
rospy.init_node("cv_bridge_test")
rospy.loginfo("Starting cv_bridge_test node")
image_converter()
rospy.spin()
except KeyboardInterrupt:
print "Shutting down cv_bridge_test node."
cv.destroyAllWindows()
效果图
2. 自主寻线
将上节的物体识别改为所寻线,运动控制左右移动/转向,剩下就是调参的事情了