measure distance in Python

[Zhefei777]

Hello, I want to measure the distance, but the Tutorial " zed_depth_sub_tutorial“ is in C++. Could anyone help me how to get the distance in Python？ Thank you

[Myzhar]

Python examples are not currently planned, but you can follow the C++ tutorial replacing the Python code to subscribe to a topic as described in the ROS wiki: http://wiki.ros.org/ROS/Tutorials/WritingPublisherSubscriber%28python%29#rospy_tutorials.2BAC8-Tutorials.2BAC8-WritingPublisherSubscriber.Writing_the_Subscriber_Node

[Zhefei777]

Thanks for your reply. I tried to write the python code, but I got all like this. � � �  � Could you tell me what I did wrong. Here is my code:

# -*- coding: utf-8 -*-
import roslib
import sys
import rospy
import cv2
import numpy as np
import math
from cv_bridge import CvBridge, CvBridgeError
from geometry_msgs.msg import Twist
from sensor_msgs.msg import Image
from std_msgs.msg import Float32

class depth_processing():

    def __init__(self):

        rospy.init_node('zed_depth', anonymous=True)

        rospy.Subscriber("/zed/depth/depth_registered", Image, self.callback)

    def callback(self, depth):
        depths = depth.data
        # print(depth.data)
        #height, width = depths[0:2]

        #v = height/2
        #u = width/2
        center = 126524
        print(depths[center])

if __name__ == '__main__': 
    try:
        detector = depth_processing()
        rospy.spin()

    except rospy.ROSInterruptException:
        rospy.loginfo("Detector node terminated.")

[Myzhar]

You must convert the data field of the Image topic to FLOAT32, so you can read the depth in meters. You can use OpenCV bridge to do it:

# -*- coding: utf-8 -*-
import roslib
import sys
import rospy
import cv2
from std_msgs.msg import String
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError
import numpy as np

class depth_processing():

    def __init__(self):

        rospy.init_node('zed_depth', anonymous=True)
        self.bridge = CvBridge()

        rospy.Subscriber("/zed/depth/depth_registered", Image, self.callback)

    def callback(self, depth_data):
        try:
            depth_image = self.bridge.imgmsg_to_cv2(depth_data, "32FC1")
        except CvBridgeError, e:
            print e

        depth_array = np.array(depth_image, dtype=np.float32)

        print('Image size: {width}x{height}'.format(width=depth_data.width,height=depth_data.height))

        u = depth_data.width/2
        v = depth_data.height/2

        print('Center depth: {dist} m'.format(dist=depth_array[u,v]))

if __name__ == '__main__': 
    try:
        detector = depth_processing()
        rospy.spin()

    except rospy.ROSInterruptException:
        rospy.loginfo("Detector node terminated.")

[Zhefei777]

Thank you for your help！ It works！

[dipampatel18]

Thank you @Myzhar for the help! I observed that the depth obtained is quite inaccurate and keeps on fluctuating a lot. But, when I switched the 'u' and 'v' in depth_array[v, u], the depth obtained was pretty accurate. Can you think of any reason since the row and column are switched?