ROS - RealsenseD435/D455 - YOLO联合_ros配置realsense仿真gazebo插件

目录

一、 RealsenseD435/D455实物 - YOLO - detect

参考

使用

二、ROS - RealsenseD435/D455 - gazebo仿真

参考

使用

三、ROS - RealsenseD435/D455 如何保存rgb与depth图

 解释

代码

四、ROS - RealsenseD435仿真 - YOLO - detect

代码

---

最终可以实现：在gazebo仿真环境中，realsenseD435相机能够检测出画面中的目标物体并且返回其(x,y,h)

一、 RealsenseD435/D455实物 - YOLO - detect

参考

realsense-D455-YOLOV5开源资料：

realsense-D455-YOLOV5

使用

win/ubuntu，装环境，插入设备，运行python realsensedetect.py，可以检测出coco数据集中的图像坐标与深度，如果想打印某目标的（x,y,h）：

label = '%s %.2f%s' % (names[int(cls)], np.mean(distance_list), 'm')
plot_one_box(xyxy, im0, label=label, color=colors[int(cls)], line_thickness=3)
c1, c2 = (int(xyxy[0]), int(xyxy[1])), (int(xyxy[2]), int(xyxy[3]))
####上面三行方便定位代码位置，下面两行是需要添加的
if names[int(cls)]=='mouse': #以鼠标为例
    print('xyh',mid_pos[0],mid_pos[1],np.mean(distance_list))

二、ROS - RealsenseD435/D455实物

参考

GitHub - IntelRealSense/realsense-ros at ros1-legacy

1、如果遇到这个错误：

(messenger-libusb.cpp:42) control_transfer returned error, index: 300, error

 检查usb线是否可用，相机是否连接正常

2、如果遇到这个错误：

[camera/realsense2_camera_manager-1] process has died [pid 11552, exit code 127, cmd /opt/ros/melodic/lib/nodelet/nodelet manager __name:=realsense2_camera_manager __log:=/home/nico/.ros/log/8a4aa5aa-f974-11ed-ac63-a333cc6ddd13/camera-realsense2_camera_manager-1.log].

ddynamic重复安装，卸载：

sudo apt-get remove ros-melodic-ddynamic-reconfigure

三、ROS - RealsenseD435/D455仿真 - gazebo

参考

源码：

IntelRealSense / realsense-ros

issaiass / realsense2_description，其中提供了支持gazebo的插件

文档

ROS1The ROS1 wrapper allows you to use Intel RealSense Depth Cameras with ROS1.These are the ROS (1) supported Distributions: Note: The latest ROS (1) release is version 2.3.2. Noetic Ninjemys (Ubuntu 20.04 Focal)Melodic Morenia (Ubuntu 18.04 Bionic)Kinetic Kame (Ubuntu 16.04 Xenial) ROS Documentation a...https://dev.intelrealsense.com/docs/ros1-wrapper

使用

运行：

roslaunch realsense2_description view_d435_model_rviz_gazebo.launch

可以看到topic：

/camera/color/image_raw

/camera/depth/image_raw

四、ROS - RealsenseD435/D455 如何保存rgb与depth图

 解释

/camera/color/image_raw 比较容易保存

/camera/depth/image_raw容易出问题，首先应该先确定编码方式

rostopic echo /camera/depth/image_raw/encoding

 bridge.imgmsg_to_cv2()函数可以将图像的ros话题转化从cv支持的格式，但容易保存成很黑的图片，参考了这篇回答

代码

#!/usr/bin/env python3
# coding:utf-8
import sys 
import rospy
import numpy as np
from sensor_msgs.msg import Image
from sensor_msgs.msg import JointState
from cv_bridge import CvBridge, CvBridgeError
import cv2
 
class Dataset_Process:
    def __init__(self):
        rospy.init_node('img_process_node', anonymous=True)
        self.bridge = CvBridge()
        rospy.Subscriber('/camera/color/image_raw', Image, self.image_raw_callback)
        rospy.Subscriber("/camera/depth/image_raw",Image, self.image_depth_callback)
     
 
    def image_raw_callback(self,data):
        self.color_image = data
        self.color_image_cv = self.bridge.imgmsg_to_cv2(data, "bgr8")
     
    def image_depth_callback(self,data):
        self.depth_image = data
        self.depth_image_cv = self.bridge.imgmsg_to_cv2(data, "16UC1")#32FC1
        self.depth_array = np.array(self.depth_image_cv, dtype=np.float32)
        self.depth_array[np.isnan(self.depth_array)] = 0
        cv2.normalize(self.depth_array, self.depth_array, 0, 1, cv2.NORM_MINMAX)
 
    def saveimg(self,imgsaving,filename):
        filestr="/home/lidia/catkin_ws/src/offboard_node/images/"+filename+".png"
        cv2.imwrite(filestr,imgsaving) 
 
if __name__ == '__main__':
 
    data_sample = Dataset_Process()
    rate = rospy.Rate(10)
    while not rospy.is_shutdown():
        rate.sleep()
        try:
            time_now_secs = int(data_sample.color_image.header.stamp.secs)
            time_now_nsecs = int(data_sample.color_image.header.stamp.nsecs)
            rgb_file_name="rgb/img"+str(time_now_secs)+"_"+str(time_now_nsecs)
            depth_file_name="depth/img"+str(time_now_secs)+"_"+str(time_now_nsecs)
            data_sample.saveimg(data_sample.color_image_cv,rgb_file_name)
            data_sample.saveimg(data_sample.depth_image_cv,depth_file_name)
            print("saving..")
        except:
            print("try again")
    rospy.spin()
 
 

四、ROS - RealsenseD435仿真 - YOLO - detect

 结合前三部分的内容，可以将第一部分中的realsensedetect.py修改成如下代码，以实现：

在gazebo仿真环境中，realsenseD435相机能够检测出画面中的目标物体并且返回其(x,y,h)

代码

import argparse
import os
import shutil
import time
from pathlib import Path
 
import cv2
import torch
import torch.backends.cudnn as cudnn
from numpy import random
import numpy as np ##
import pyrealsense2 as rs ##
 
from models.experimental import attempt_load
from utils.general import (
    check_img_size, non_max_suppression, apply_classifier, scale_coords,
    xyxy2xywh, plot_one_box, strip_optimizer, set_logging)
from utils.torch_utils import select_device, load_classifier, time_synchronized
from utils.datasets import letterbox
import sys 
import rospy
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError
import cv2
 
class Dataset_Process:
    def __init__(self):
        rospy.init_node('img_process_node', anonymous=True)
        self.bridge = CvBridge()
        rospy.Subscriber('/camera/color/image_raw', Image, self.image_raw_callback)
        rospy.Subscriber("/camera/depth/image_raw",Image, self.image_depth_callback)
        self.detect()
        # rospy.Subscriber("/joint_states",JointState,self.joint_state_callback)
    
    def image_raw_callback(self,data):
        self.color_image = data
        self.color_image_cv = self.bridge.imgmsg_to_cv2(data, "bgr8")
        # cv2.imshow("frame" , cv_img)
        # cv2.waitKey(1)
        # filestr="/home/lidia/catkin_ws/src/offboard_node/images/rgb/img"+str(data.header.stamp.secs)+"_"+str(data.header.stamp.nsecs)+".png"
        # cv2.imwrite(filestr,cv_img)
 
    def image_depth_callback(self,data):
        self.depth_image = data
        self.depth_image_cv = self.bridge.imgmsg_to_cv2(data, "16UC1")# 32FC1
        self.depth_array = np.array(self.depth_image_cv, dtype=np.float32)
        self.depth_array[np.isnan(self.depth_array)]=0
        cv2.normalize(self.depth_array,self.depth_array,0,1,cv2.NORM_MINMAX)
 
 
    def detect(self,save_img=False):
        out, source, weights, view_img, save_txt, imgsz = \
            opt.save_dir, opt.source, opt.weights, opt.view_img, opt.save_txt, opt.img_size
        webcam = source == '0' or source.startswith(('rtsp://', 'rtmp://', 'http://')) or source.endswith('.txt')
 
        # Initialize
        set_logging()
        device = select_device(opt.device)
        if os.path.exists(out):  # output dir
            shutil.rmtree(out)  # delete dir
        os.makedirs(out)  # make new dir
        half = device.type != 'cpu'  # half precision only supported on CUDA
 
        # Load model
        model = attempt_load(weights, map_location=device)  # load FP32 model,载入模型
        imgsz = check_img_size(imgsz, s=model.stride.max())  # check img_size
        if half:
            model.half()  # to FP16
        # Set Dataloader
        vid_path, vid_writer = None, None
        view_img = True
        cudnn.benchmark = True  # set True to speed up constant image size inference
        #dataset = LoadStreams(source, img_size=imgsz)
 
        # Get names and colors
        names = model.module.names if hasattr(model, 'module') else model.names
        colors = [[random.randint(0, 255) for _ in range(3)] for _ in range(len(names))]
 
        # Run inference
        t0 = time.time()
        img = torch.zeros((1, 3, imgsz, imgsz), device=device)  # init img
        _ = model(img.half() if half else img) if device.type != 'cpu' else None  # run once
        # pipeline = rs.pipeline()
        # 创建 config 对象：
        config = rs.config()
        # config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
        config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 60)
        config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 60)
 
        # Start streaming
        # pipeline.start(config)
        align_to_color = rs.align(rs.stream.color)
        while True:
            start = time.time()
            # Wait for a coherent pair of frames（一对连贯的帧）: depth and color
            # frames = pipeline.wait_for_frames()
            # frames = align_to_color.process(frames)
            # depth_frame = frames.get_depth_frame()  # 深度
            # color_frame = frames.get_color_frame()  # rgb
            # print('depth_frame',type(depth_frame))  # <class 'pyrealsense2.pyrealsense2.depth_frame'>
            # print('color_frame',type(color_frame))  # <class 'pyrealsense2.pyrealsense2.video_frame'>
            # color_image = np.asanyarray(color_frame.get_data())
            # depth_image = np.asanyarray(depth_frame.get_data())
            # mask = np.zeros([color_image.shape[0], color_image.shape[1]], dtype=np.uint8)
            # mask[0:480, 320:640] = 255
 
            sources = [source]
            imgs = [None]
            path = sources
            # imgs[0] = color_image  # 检测是基于rgb图检测的
            # im0s = imgs.copy()
            # img = [letterbox(x, new_shape=imgsz)[0] for x in im0s]
            # img = np.stack(img, 0)
            # img = img[:, :, :, ::-1].transpose(0, 3, 1, 2)  # BGR to RGB, to 3x416x416, uint8 to float32
            # img = np.ascontiguousarray(img, dtype=np.float16 if half else np.float32)
            # img /= 255.0  # 0 - 255 to 0.0 - 1.0
            depth_image = self.depth_array
 
            imgs[0] = self.color_image_cv
            im0s = imgs.copy()
            img = [letterbox(x, new_shape=imgsz)[0] for x in im0s]
            # img = [letterbox(x, new_shape=imgsz)[0] for x in img]
            img = np.stack(img, 0)
            # print(img.shape)
            img = img.transpose(0, 3, 1, 2) # BGR to RGB, to 3x416x416, uint8 to float32
            img = np.ascontiguousarray(img, dtype=np.float16 if half else np.float32)
            img /=255.0
            
 
 
            # Get detections
            img = torch.from_numpy(img).to(device)  # 转化成了torch的格式传入model
            if img.ndimension() == 3:
                img = img.unsqueeze(0)
            t1 = time_synchronized()
            pred = model(img, augment=opt.augment)[0]
 
            # Apply NMS
            pred = non_max_suppression(pred, opt.conf_thres, opt.iou_thres, classes=opt.classes, agnostic=opt.agnostic_nms)
            t2 = time_synchronized()
 
            for i, det in enumerate(pred):  # detections per image
                p, s, im0 = path[i], '%g: ' % i, im0s[i].copy()
                s += '%gx%g ' % img.shape[2:]  # print string
                gn = torch.tensor(im0.shape)[[1, 0, 1, 0]]  # normalization gain whwh
                if det is not None and len(det):
                    # Rescale boxes from img_size to im0 size
                    det[:, :4] = scale_coords(img.shape[2:], det[:, :4], im0.shape).round()
 
                    # Print results
                    for c in det[:, -1].unique():
                        n = (det[:, -1] == c).sum()  # detections per class
                        s += '%g %ss, ' % (n, names[int(c)])  # add to string，存放检测到了几个物体
 
                    # Write results
                    for *xyxy, conf, cls in reversed(det):
                        xywh = (xyxy2xywh(torch.tensor(xyxy).view(1, 4)) / gn).view(-1).tolist()  # normalized xywh
                        line = (cls, conf, *xywh) if opt.save_conf else (cls, *xywh)  # label format
                        distance_list = []
                        mid_pos = [int((int(xyxy[0]) + int(xyxy[2])) / 2), int((int(xyxy[1]) + int(xyxy[3])) / 2)]  # 确定索引深度的中心像素位置左上角和右下角相加在/2
                        min_val = min(abs(int(xyxy[2]) - int(xyxy[0])), abs(int(xyxy[3]) - int(xyxy[1])))  # 确定深度搜索范围
                        # print(box,)
                        randnum = 40
                        for i in range(randnum):
                            bias = random.randint(-min_val // 4, min_val // 4)
                            dist = depth_image[int(mid_pos[0] + bias), int(mid_pos[1] + bias)]###照片的xy位置
                            # print(max())
                            # print(int(mid_pos[1] + bias), int(mid_pos[0] + bias))
                            if dist:
                                distance_list.append(dist)
                        distance_list = np.array(distance_list)
                        distance_list = np.sort(distance_list)[randnum // 2 - randnum // 4:randnum // 2 + randnum // 4]  # 冒泡排序+中值滤波
 
                        label = '%s %.2f%s' % (names[int(cls)], np.mean(distance_list), 'm')#label中存放着标签的名字和距离
                        plot_one_box(xyxy, im0, label=label, color=colors[int(cls)], line_thickness=3)
                        c1, c2 = (int(xyxy[0]), int(xyxy[1])), (int(xyxy[2]), int(xyxy[3]))
                        # print(c1, '*******',c2 )
                        if names[int(cls)]=='airplane':
                            print('xyh',mid_pos[0],mid_pos[1],np.mean(distance_list))#如果是鼠标就返回xyh
                        # print(label,type(label))
 
                # Print time (inference + NMS)
                print('%sDone. (%.3fs)' % (s, t2 - t1))
 
                # Stream results
                if view_img:
                    cv2.imshow(p, im0)
                    if cv2.waitKey(1) == ord('q'):  # q to quit
                        raise StopIteration
        # print('Done. (%.3fs)' % (time.time() - t0))
 
 
if __name__ == '__main__':
    parser = argparse.ArgumentParser()
    parser.add_argument('--weights', nargs='+', type=str, default='yolov5l.pt', help='model.pt path(s)')#在第一次执行的时候，会下载yolov5m.pt，是一个训练好的模型
    parser.add_argument('--source', type=str, default='inference/images', help='source')  # file/folder, 0 for webcam
    parser.add_argument('--img-size', type=int, default=640, help='inference size (pixels)')
    parser.add_argument('--conf-thres', type=float, default=0.25, help='object confidence threshold')
    parser.add_argument('--iou-thres', type=float, default=0.45, help='IOU threshold for NMS')
    parser.add_argument('--device', default='', help='cuda device, i.e. 0 or 0,1,2,3 or cpu')
    parser.add_argument('--view-img', action='store_true', help='display results')
    parser.add_argument('--save-txt', action='store_true', help='save results to *.txt')
    parser.add_argument('--save-conf', action='store_true', help='save confidences in --save-txt labels')
    parser.add_argument('--save-dir', type=str, default='inference/output', help='directory to save results')
    parser.add_argument('--classes', nargs='+', type=int, help='filter by class: --class 0, or --class 0 2 3')
    parser.add_argument('--agnostic-nms', action='store_true', help='class-agnostic NMS')
    parser.add_argument('--augment', action='store_true', help='augmented inference')
    parser.add_argument('--update', action='store_true', help='update all models')
    opt = parser.parse_args()
    #print('***************',opt)
 
    with torch.no_grad(): # 一个上下文管理器，被该语句wrap起来的部分将不会track梯度
        data_sample = Dataset_Process()