0
$\begingroup$

Rosanswers logo

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


Originally posted by amasiero on ROS Answers with karma: 11 on 2015-10-12

Post score: 1

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

The line

depth_image = self.bridge.imgmsg_to_cv2(depth_data, "32FC1")

change the code to

depth_image = self.bridge.imgmsg_to_cv2(depth_data, "passthrough")

then, you'll get depth_image as float32.

(no longer transform using np.array is not needed.)


Originally posted by taka084 with karma: 16 on 2016-12-26

This answer was ACCEPTED on the original site

Post score: 0

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.