Depth image from rectified stereo images or disparity image

edit

I need a depth image for my project(MoveIt robotic arm with CV). Stereo_image_proc package provides rectified stereo images and disparity image. I can view them in RVIZ and rqt_image_view.

What I have tried so far

1)As referenced here https://answers.ros.org/question/1658... , I've tried using disparity_image_proc package. My launch file looks like that

<launch>
  <node pkg="nodelet" type="nodelet" name="stereo_proc"  args="manager"/>
  <node name="disparity_image_proc" pkg="nodelet" type="nodelet" args="load disparity_image_proc/depth_image stereo_proc" output="screen">
    <remap from="/elp/right/camera_info" to="/right/camera_info"/>
    <remap from="/elp/left/camera_info" to="/left/camera_info"/>
    <remap from="/elp/left/image_rect_color" to="/left/image_rect_color"/>
    <remap from="/elp/disparity" to="/disparity"/>
    <remap from="/depth_image" to="/depth_image"/> 
    <param name="approximate_sync" value="true" type="bool"/>
    <param name="queue_size" value="20" type="int"/> 
  </node>
</launch>

I can launch the nodelet, it doesn't produce any errors, but the /depth_image topic is empty.

2)I have tried writing my own Python node that would subscribe to the pair of rectified images and output a depth image. Here's the code I've came up so far

#!/usr/bin/env python

from __future__ import print_function

import roslib
roslib.load_manifest('depth')
import sys
import rospy
import cv2
from std_msgs.msg import String
from sensor_msgs.msg import Image
import message_filters
from cv_bridge import CvBridge, CvBridgeError

class image_converter:

  def __init__(self):
    self.image_pub = rospy.Publisher("depth",Image,queue_size=5)

    self.bridge = CvBridge()

    print("Create subscribers for each topic")
    self.left = message_filters.Subscriber("/elp/left/image_rect_color", Image)
    self.right = message_filters.Subscriber("/elp/right/image_rect_color", Image)
    print("Create sync filter. Use exact or approximate as appropriate.")
    self.ts = message_filters.ApproximateTimeSynchronizer([self.left, self.right], queue_size=5, slop=0.1)
    print("Registering callback")
    self.ts.registerCallback(self.callback)

  def callback(self,left,right):
    try:
      cv_image_left = self.bridge.imgmsg_to_cv2(left, "rgb8")
      cv_image_right = self.bridge.imgmsg_to_cv2(right, "rgb8")
      cv_image_left_new = cv2.cvtColor(cv_image_left, cv2.COLOR_BGR2GRAY)
      cv_image_right_new = cv2.cvtColor(cv_image_right, cv2.COLOR_BGR2GRAY)
    except CvBridgeError as e:
      print(e)
    print("creating stereo image")
    stereo = cv2.StereoBM_create(numDisparities=16, blockSize=15)
    depth = stereo.compute(cv_image_left_new, cv_image_right_new)
    print("showing depth image")
    cv2.imshow("Image window", depth)
    cv2.waitKey(3)

    try:
      self.image_pub.publish(self.bridge.cv2_to_imgmsg(depth, "16SC1"))
    except CvBridgeError as e:
      print(e)


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

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

It runs without errors, but still nothing on the /depth topic. The image_view window hangs after a while. For this one I am not sure if it is something fundamentally wrong with this approach or I just used wrong image formats.

Any help and feedback would be very much appreciated. Another solution for this problem(obtaining depth image from stereo image OR disparity image)would also be very welcome.

Update 1: Here's my rqt_graph with camera driver(elp camera driver), stereo_image_proc and disaprity_image_proc running.