机器人感知避障-基于YOLOv8-seg和ROS2（Python实现）

简介

感知避障通常是机器人研发中的一项重要任务。在复合机器人（底盘AGV+躯干+机械臂）或人形机器人在实际场景的行进过程中通常采用多传感器进行三维空间中机身的避障，针对有色液体、宠物粪便、阶梯、悬崖等，采用感知避障。

算法思路

1、相机取流，一般选用RGBD相机获取color和depth
2、在2D图像上进行分割
3、将分割得到的mask映射到深度图
4、根据深度图获取点云
5、将点云发出

依赖

PyTorch、Open3D、ROS2、Realsense ROS驱动

代码

import os
import numpy as np
import cv2
import open3d as o3d
from datetime import datetime
import torch
from ultralytics import  YOLO

import rclpy
from sensor_msgs.msg import PointCloud2, PointField, Image, CameraInfo

from cv_bridge import CvBridge
from std_msgs.msg import Header
from rclpy.node import Node

def depth_to_pointcloud(depth_image, fx, fy, cx, cy):
    # Create Open3D Image from depth map
    o3d_depth = o3d.geometry.Image(depth_image)

    # Get intrinsic parameters
    # fx, fy, cx, cy = intrinsic.fx, intrinsic.fy, intrinsic.ppx, intrinsic.ppy

    # Create Open3D PinholeCameraIntrinsic object
    o3d_intrinsic = o3d.camera.PinholeCameraIntrinsic(width=depth_image.shape[1], height=depth_image.shape[0], fx=fx, fy=fy, cx=cx, cy=cy)

    # Create Open3D PointCloud object from depth image and intrinsic parameters
    pcd = o3d.geometry.PointCloud.create_from_depth_image(o3d_depth, o3d_intrinsic)

    return pcd

def achieve_targetpointcloud(mask, depth, fx, fy, cx, cy):
    mask_resized = cv2.resize(mask, (depth.shape[1], depth.shape[