机器人感知算法-基于yolov5和ROS2的物体识别定位（Python实现）

简介

视觉感知是机器人研发的重要一环。机械臂对物体进行抓取、放置、操作三类任务中，需要对感兴趣的物体进行识别定位，从而为机械臂的控制规划提供视觉指导信息。这里提供一份示例代码，以供研究者和工程师交流。

算法思路

1、相机取流，一般选用RGBD相机获取color和depth
2、yolov5进行目标识别
3、识别的bounding box映射到depth，并将相关区域的深度图转为点云
4、对目标点云进行滤波处理，并估计位置信息
5、通过ROS2通信模块将目标的xyz信息发送出去

依赖

PyTorch、Open3D、ROS2、Realsense相机驱动

代码

import os
import torch
import cv2
import numpy as np
import open3d as o3d
import pyrealsense2 as rs
import math

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Transform
from std_msgs.msg import Header
from vision_msgs.msg import DetectedObject, DetectedObjectList
from datetime import datetime

camera_to_base = np.array([[0.4830191777293635, -0.5454382934019704, 0.6849741177857716, 0.09830437700902608],
                           [-0.3508700489363267, -0.8372981068953109, -0.4193114462412742, 0.5652720778206468],
                           [0.8022360517360645, -0.03780143225164029, -0.5958090038046218, 0.794058946750924],
                           [0.0, 0.0, 0.0, 1.0]])

objectname = {
    0:"push pedal",
    1:"cup",
    2:"doorknob"
}


def object_2D_detection(img):
    # Model
    model = torch.hub.load(r'/home/lz/yolov5', 'custom', path=r'runs/tra