ROS D435I recognizes the target and acquires depth data

ROS D435I recognizes the target and acquires depth data

Use the D435I camera, and obtain the color image and matched depth data based on ros, and use OPENCV to perform target recognition on the color image to obtain the pixel range of the target, and then obtain the depth data The key point is: convert the ros image data to opencv format, and
get The actual depth value of the target pixel

1. d435i start and modify

roslaunch realsense2_camera rs_camera_vins.launch 

Use the above command to start d435i, and you can modify and match the resolution in it. Among them color_widthand color_heightis the resolution of the color image, you can choose 720 480, 1280 720 and 1920*1080. Modify align_depththe depth matching to obtain the matched depth image.

rqt_image_view

Use the above command to view the image, you can see the color image and deep image of the required resolution. It can be seen from the depth image that when the distance is very close or far away, the depth data is black. The color image is in bgr8 format, and the depth image is in 16UC1 format, which can be obtained through Image.encoding.

rostopic echo /camera/aligned_depth_to_color/camera_info

Using the above command, you can view the parameters of the image, including resolution and internal reference.

2. The ros image is converted to opencv for processing

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()

Using the above code, the acquired image can be converted into a three-channel bgr opencv image, which can be processed directly with opencv (but I cannot cv2.imshow), and the released image can be viewed with rqt.

3. Obtain the actual depth value
   As mentioned above, the format of the depth value is 16UC1. After converting it into opencv, you can directly get the corresponding value through the array, and the unit is 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)

The above code converts the depth graphics to the format of opencv, and it is the encoding method of 16UC1. The read shapes are 720 and 1280 respectively, and print(depth_img[depth_cy, depth_cx])the depth value can be obtained by passing it.

4. Publish your own topic and carry out transmission between ros
   . It is too troublesome to create a topic data structure by yourself. It is better to use the existing one. Use from geometry_msgs.msg import Point, this data with xyz is just used for representation.
   There are two schemes, xy represents the target pixel coordinates in the image or the pitch and yaw paranoia after conversion, z represents the depth data; or the coordinate order on the left front, the x axis represents the paranoia of yaw, z represents the paranoia of pitch (will be converted later into a high degree of paranoia), y represents the depth data.
   Use the second option here

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)