#! /home/ubuntu/anaconda3/envs/yolo3/bin/python
# -*- coding: utf-8 -*-
# 定义编码，中文注释
import numpy as np
import rospy
import os
import time
import matplotlib.pyplot as plt
import sys
from sensor_msgs.msg import Image, CameraInfo
sys.path.remove("/opt/ros/kinetic/lib/python2.7/dist-packages")
sys.path.append("/home/ubuntu/cv_bridge_ws/install/lib/python3/dist-packages/")
from cv_bridge import CvBridge, CvBridgeError
from geometry_msgs.msg import Pose
import cv2
plt.rcParams['font.sans-serif'] = ['SimHei'] # 用来正常显示中文标签
plt.rcParams['axes.unicode_minus'] = False # 用来正常显示负号
# 参数
yolo_dir = "./darknet" # YOLO文件路径
CONFIDENCE = 0.5 # 过滤弱检测的最小概率
THRESHOLD = 0.4 # 非最大值抑制阈值
class ImageConverter():
def __init__(self):
# 创建图像缓存相关的变量
self.cv_image=None
self.get_image=False
# 创建ROS中cv_bridge
self.bridge = CvBridge()
self.Info = np.zeros((3,3),float)
self.camera_co = [1000,10000,1000]
self.Obstance = Pose()
# 声明图像的发布者和订阅者
self.image_pub = rospy.Publisher("yolov3_image_image", Image, queue_size=1)
self.pose_pub1 = rospy.Publisher("obstance_pose2", Pose, queue_size=1)
# self.pose_pub1 = rospy.Publisher("object_detect_pose1", Pose, queue_size=1)
# self.pose_pub2 = rospy.Publisher("object_detect_pose2", Pose, queue_size=1)
self.camera_info_sub = rospy.Subscriber("/robot2/camera_info", CameraInfo, self.camera_Info, queue_size=1)
self.image_sub = rospy.Subscriber("/robot2/image_raw", Image, self.image_callback, queue_size=1)
def image_callback(self , data):
# 判断当前图像是否处理完
if not self.get_image:
# 使用cv_bridge将ROS的图像数据转换成OpenCv的图像格式
try:
self.cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
#self.cv_image =cv2.imread("/home/ubuntu/robot_ws/src/darknet/data/dog.jpg")
# 设置标志 表示收到图像
self.get_image=True
print(self.get_image)
def camera_Info(self, msg):
#camera paramate
self.Info[0][0] = msg.K[0]
self.Info[0][1] = msg.K[1]
self.Info[0][2] = msg.K[2]
self.Info[1][0] = msg.K[3]
self.Info[1][1] = msg.K[4]
self.Info[1][2] = msg.K[5]
self.Info[2][0] = msg.K[6]
self.Info[2][1] = msg.K[7]
self.Info[2][2] = msg.K[8]
def yolov3_image(self):
weightsPath = "/home/ubuntu/darknet/yolov3.weights"#os.path.join(yolo_dir, 'yolov3.weights') # 权重文件
configPath = "/home/ubuntu/darknet/cfg/yolov3.cfg"#os.path.join("./darknet/cfg", 'yolov3.cfg')
# 配置文件
labelsPath = os.path.join("/home/ubuntu/darknet/data", 'coco.names') # label名称
# 加载网络、配置权重
net = cv2.dnn.readNetFromDarknet(configPath, weightsPath) # 利用下载的文件
print("[INFO] loading YOLO from disk...") # 可以打印下信息
# net需要的输入是blob格式的，用blobFromImage这个函数来转格式
blobImg = cv2.dnn.blobFromImage(self.cv_image, 1.0 / 255.0, (416, 416), None, True,False)
net.setInput(blobImg) # 调用setInput函数将图片送入输入层
# 获取网络输出层信息（所有输出层的名字），设定并前向传播
# yolo在每个scale都有输出，outInfo是每个scale的名字信息，供net.forward使用
outInfo = net.getUnconnectedOutLayersNames()
start = time.time()
layerOutputs = net.forward(outInfo) # 得到各个输出层的、各个检测框等信息，是二维结构。
end = time.time()
print("[INFO] YOLO took {:.6f} seconds".format(end - start)) # 可以打印下信息
# 拿到图片尺寸
(H, W) = self.cv_image.shape[:2]
cv2.imshow("cv_image",self.cv_image)
# 过滤layerOutputs
# layerOutputs的第1维的元素内容: [center_x, center_y, width, height, objectness, N-class score data]
# 过滤后的结果放入：
boxes = [] # 所有边界框（各层结果放一起）
confidences = [] # 所有置信度
classIDs = [] # 所有分类ID
# 1）过滤掉置信度低的框框
for out in layerOutputs: # 各个输出层
for detection in out: # 各个框框
# 拿到置信度
scores = detection[5:] # 各个类别的置信度
classID = np.argmax(scores) # 最高置信度的id即为分类id
confidence = scores[classID] # 拿到置信度
# 根据置信度筛查
if confidence > CONFIDENCE:
box = detection[0:4] * np.array([W, H, W, H]) # 将边界框放会图片尺寸
(centerX, centerY, width, height) = box.astype("int")
x = int(centerX - (width / 2))
y = int(centerY - (height / 2))
boxes.append([x, y, int(width), int(height)])
confidences.append(float(confidence))
classIDs.append(classID)
# 2）应用非最大值抑制(non-maxima suppression，nms)进一步筛掉
idxs = cv2.dnn.NMSBoxes(boxes, confidences, CONFIDENCE, THRESHOLD) # boxes中，保留的box的索引index存入idxs
# 得到labels列表
with open(labelsPath, 'rt') as f:
labels = f.read().rstrip('\n').split('\n')
# 应用检测结果
np.random.seed(42)
COLORS = np.random.randint(0, 255, size=(len(labels), 3),
dtype="uint8") # 框框显示颜色，每一类有不同的颜色，每种颜色都是由RGB三个值组成的，所以size为(len(labels), 3)
print("idxs", len(idxs))
if len(idxs) > 0:
for i in idxs.flatten(): # indxs是二维的，第0维是输出层，所以这里把它展平成1维
(x, y) = (boxes[i][0], boxes[i][1])
(w, h) = (boxes[i][2], boxes[i][3])
color = [int(c) for c in COLORS[classIDs[i]]]
cv2.rectangle(self.cv_image, (x, y), (x + w, y + h), color, 2)
print("x:",x)
# 线条粗细为2px
text = "{}: {:.4f}".format(labels[classIDs[i]], confidences[i])
print("[INFO] predect result is: ", text)
(text_w, text_h), baseline = cv2.getTextSize(text, cv2.FONT_HERSHEY_SIMPLEX, 0.5, 2)
cv2.rectangle(self.cv_image, (x, y - text_h - baseline), (x + text_w, y), color, -1)
# cv.FONT_HERSHEY_SIMPLEX字体风格、0.5字体大小、粗细2px
cv2.putText(self.cv_image, text, (x, y - 5), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 0), 2)
try:
self.image_pub.publish(self.bridge.cv2_to_imgmsg(self.cv_image, "bgr8"))
print("yolo - out")
except CvBridgeError as e:
print(e)
# 判断图片中障碍物位置
#ni ju zhen
Info_ni = np.linalg.inv(np.array(self.Info))
Centerx = x + w/2
Centery = y + h/2
D_co=np.array([Centerx, Centery, 1])
camera_co0 = Info_ni.dot(D_co)
object_height = 1.0
depth = (object_height*self.Info[1][1])/h
camera_co0 = depth * camera_co0
#
if self.camera_co[2] > camera_co0[2]:
self.camera_co = camera_co0
self.Obstance.position.x = self.camera_co[0]*0.001
self.Obstance.position.y = self.camera_co[1]*0.001
self.Obstance.position.z = self.camera_co[2
- 1
- 2
前往页