Le ROS D435I reconnaît la cible et acquiert les données de profondeur

Le ROS D435I reconnaît la cible et acquiert les données de profondeur

Utilisez la caméra D435I et obtenez l'image couleur et les données de profondeur correspondantes en fonction de ros, et utilisez OPENCV pour effectuer la reconnaissance de la cible sur l'image couleur afin d'obtenir la plage de pixels de la cible, puis obtenez les données de profondeur Le point clé est : convertir les données d'image ros au format opencv et
obtenez la valeur de profondeur réelle du pixel cible

1. d435i démarrer et modifier

roslaunch realsense2_camera rs_camera_vins.launch 

Utilisez la commande ci-dessus pour démarrer d435i, et vous pouvez modifier et faire correspondre la résolution qu'elle contient. Parmi eux color_widthet color_heightest la résolution de l'image couleur, vous pouvez choisir 720 480, 1280 720 et 1920*1080. Modifiez align_depthla correspondance de profondeur pour obtenir l'image de profondeur correspondante.

rqt_image_view

Utilisez la commande ci-dessus pour afficher l'image, vous pouvez voir l'image couleur et l'image profonde de la résolution requise. On peut voir sur l'image de profondeur que lorsque la distance est très proche ou très éloignée, les données de profondeur sont noires. L'image couleur est au format bgr8 et l'image de profondeur au format 16UC1, qui peut être obtenue via Image.encoding.

rostopic echo /camera/aligned_depth_to_color/camera_info

À l'aide de la commande ci-dessus, vous pouvez afficher les paramètres de l'image, y compris la résolution et la référence interne.

2. L'image ros est convertie en opencv pour le traitement

import rospy
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError

rospy.init_node('vins_img')
rospy.Subscriber("/camera/color/image_raw",                                         Image, vins_img_callback,queue_size=1)
bridge = CvBridge()
image_pub = rospy.Publisher("cv_bridge_image", Image, queue_size=1)

def vins_img_callback(data = Image):
    try:
        cv_img = bridge.imgmsg_to_cv2(data, "bgr8")
    except CvBridgeError as e:
        print(e)
        
final_img = ……

    try:
        image_pub.publish(bridge.cv2_to_imgmsg(final_img, "bgr8"))
    except CvBridgeError as e:
        print(e)

    pass

while not rospy.is_shutdown():
    rospy.spin()

En utilisant le code ci-dessus, l'image acquise peut être convertie en une image bgr opencv à trois canaux, qui peut être traitée directement avec opencv (mais je ne peux pas cv2.imshow), et l'image publiée peut être visualisée avec rqt.

3. Obtenir la valeur de profondeur réelle
   Comme mentionné ci-dessus, le format de la valeur de profondeur est 16UC1. Après l'avoir convertie en opencv, vous pouvez directement obtenir la valeur correspondante via le tableau, et l'unité est mm.

def vins_dep_callback(data = Image):
    global depth_img
    try:
        depth_img = bridge.imgmsg_to_cv2(data, "16UC1")
    except CvBridgeError as e:
        print(e)

Le code ci-dessus convertit les graphiques de profondeur au format opencv, et c'est la méthode d'encodage de 16UC1. Les formes lues sont respectivement 720 et 1280, et print(depth_img[depth_cy, depth_cx])la valeur de profondeur peut être obtenue en la passant.

4. Publiez votre propre sujet et effectuez la transmission entre ros
   . Il est trop fastidieux de créer une structure de données de sujet par vous-même. Il est préférable d'utiliser celle qui existe déjà. Utilisez from geometry_msgs.msg import Point, ces données avec xyz ne sont utilisées que pour la représentation.
   Il existe deux schémas, xy représente les coordonnées du pixel cible dans l'image ou la paranoïa de tangage et de lacet après conversion, z représente les données de profondeur ; ou l'ordre des coordonnées sur le devant gauche, l'axe des x représente la paranoïa de lacet, z représente la paranoïa de hauteur (sera converti plus tard en un haut degré de paranoïa), y représente les données de profondeur.
   Utilisez la deuxième option ici

from geometry_msgs.msg import Point
obj_pub = rospy.Publisher("obj_point",  Point, queue_size=1 )

    obj_dat  = Point()
    obj_dat.x = det_yaw
    obj_dat.y = det_pitch
    obj_dat.z = float(depth_img[depth_cy, depth_cx])/1000
    obj_pub.publish(obj_dat)