Getting depth information from point using Python

edit

Hi guys,

I'm trying to get depth information from a specific point. I'm using openni_launch topics "camera/rgb/image_color" (to calculate the point) and "camera/depth/image" (to get depth information). I use Python as a programming language.

When I use C++ language, I have a generic function called "at<type>(x, y)". However, I don't know how to translate this for Python.

Code:

#!/usr/bin/env python
import roslib; roslib.load_manifest('rfh_follow_me')
import sys
import rospy
import cv2
import cv2.cv
import message_filters
import numpy as np
from std_msgs.msg import String
from sensor_msgs.msg import Image
from geometry_msgs.msg import Twist
from p2os_msgs.msg import MotorState
from cv_bridge import CvBridge, CvBridgeError
from datetime import datetime

class follow_person:
        def __init__(self):
        self.bridge = CvBridge()

        image_sub = message_filters.Subscriber("camera/rgb/image_color", Image)
        depth_sub = message_filters.Subscriber("camera/depth/image", Image)

        self.ts = message_filters.ApproximateTimeSynchronizer([image_sub, depth_sub], 10, 0.5)
        self.ts.registerCallback(self.callback)

        self.body_cascade = cv2.CascadeClassifier('/usr/local/share/OpenCV/haarcascades/haarcascade_upperbody.xml')
        self.cmd_vel_pub = rospy.Publisher("/cmd_vel", Twist, queue_size=10)

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

        depth_array = np.array(depth_image, dtype=np.float32)
        cv2.normalize(depth_array, depth_array, 0, 1, cv2.NORM_MINMAX)

        gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
        upper_bodys = self.body_cascade.detectMultiScale(
            gray,
            scaleFactor=1.1,
            minNeighbors=10,
            minSize=(100,100),
            flags=cv2.cv.CV_HAAR_SCALE_IMAGE
        )

        for (x, y, w, h) in upper_bodys:
            cv2.rectangle(depth_array, (x, y), (x+w, y+h), (0,255,0), 2)
            cv2.circle(depth_array, (x+(w/2), h), 3, (171,110,0), 2)

        cv2.imshow('Body Recognition', depth_array)
        cv2.waitKey(3)

def main(args):
    fp = follow_person()
    rospy.init_node('follow_person', anonymous=True)
    try:
        rospy.spin()
    except KeyboardInterrupt:
        print "Shutting down"
    cv2.destroyAllWindows()

if __name__ == '__main__':
    main(sys.argv)

The point that I'm looking for depth information is the one inside cv2.circle function.

Can anyone help me, please?

Best regards, Andrey