camera_depth.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 #
00004 # ROS node to provide access to the camera by wrapping NaoQI access (may not
00005 # be the most efficient way...)
00006 #
00007 # Copyright 2012 Daniel Maier, University of Freiburg
00008 # Copyright 2014 Aldebaran Robotics
00009 # http://www.ros.org/wiki/nao
00010 #
00011 # Redistribution and use in source and binary forms, with or without
00012 # modification, are permitted provided that the following conditions are met:
00013 #
00014 #    # Redistributions of source code must retain the above copyright
00015 #       notice, this list of conditions and the following disclaimer.
00016 #    # Redistributions in binary form must reproduce the above copyright
00017 #       notice, this list of conditions and the following disclaimer in the
00018 #       documentation and/or other materials provided with the distribution.
00019 #    # Neither the name of the University of Freiburg nor the names of its
00020 #       contributors may be used to endorse or promote products derived from
00021 #       this software without specific prior written permission.
00022 #
00023 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00024 # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00025 # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00026 # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00027 # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00028 # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00029 # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00030 # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00031 # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00032 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033 # POSSIBILITY OF SUCH DAMAGE.
00034 #
00035 
00036 from collections import defaultdict
00037 import rospy
00038 from sensor_msgs.msg import Image, CameraInfo
00039 from naoqi_driver.naoqi_node import NaoqiNode
00040 import camera_info_manager
00041 from sensor_msgs.msg._CameraInfo import CameraInfo
00042 
00043 from naoqi import ALProxy
00044 
00045 from cv_bridge import CvBridge, CvBridgeError
00046 import cv2
00047 import numpy as np
00048 
00049 # import color spaces
00050 from naoqi_sensors.vision_definitions import kDepthColorSpace
00051 
00052 class NaoCam (NaoqiNode):
00053     def __init__(self):
00054         NaoqiNode.__init__(self, 'nao_depth')
00055 
00056         self.camProxy = self.get_proxy("ALVideoDevice")
00057         if self.camProxy is None:
00058             exit(1)
00059         self.nameId = None
00060         self.camera_infos = {}
00061         def returnNone():
00062             return None
00063 
00064         self.bridge = CvBridge()
00065         # ROS publishers
00066         self.pub_img_ = rospy.Publisher('~image_raw', Image)
00067         self.pub_cimg_ = rospy.Publisher('~image_color', Image)
00068         self.pub_info_ = rospy.Publisher('~camera_info', CameraInfo)
00069 
00070         # initialize the parameter of the depth camera
00071         self.setup_cam()
00072 
00073     def setup_cam( self):
00074         # unsubscribe for all zombie subscribers
00075         self.camProxy.unsubscribeAllInstances("rospy_gvm")
00076         # subscribe
00077         kDepthCamera = 2
00078         self.frame_id = rospy.get_param("~frame_id", "/CameraDepth_frame")
00079         resolution = rospy.get_param("~resolution", 1)  #320x240
00080         framerate  = rospy.get_param("~fps", 15)
00081         color_space= rospy.get_param("~color_space", 17) # mono16
00082         self.nameId = self.camProxy.subscribeCamera("rospy_gvm", kDepthCamera, resolution, color_space, framerate)
00083         rospy.loginfo('Using camera: depth camera. Subscriber name is %s .' % (self.nameId))
00084 
00085         return
00086 
00087     def main_loop(self):
00088         img = Image()
00089         cimg = Image()
00090         r = rospy.Rate(15)
00091         while not rospy.is_shutdown():
00092             if self.pub_img_.get_num_connections() == 0:
00093                 r.sleep()
00094                 continue
00095 
00096             image = self.camProxy.getImageRemote(self.nameId)
00097             if image is None:
00098                 continue
00099             # Deal with the image
00100             '''
00101             #Images received from NAO have
00102             if self.config['use_ros_time']:
00103                 img.header.stamp = rospy.Time.now()
00104             else:
00105                 img.header.stamp = rospy.Time(image[4] + image[5]*1e-6)
00106             '''
00107             img.header.stamp = rospy.Time.now()
00108             img.header.frame_id = self.frame_id
00109             img.height = image[1]
00110             img.width = image[0]
00111             nbLayers = image[2]
00112             if image[3] == kDepthColorSpace:
00113                 encoding = "mono16"
00114             else:
00115                 rospy.logerr("Received unknown encoding: {0}".format(image[3]))
00116             img.encoding = encoding
00117             img.step = img.width * nbLayers
00118             img.data = image[6]
00119 
00120             self.pub_img_.publish(img)
00121             
00122             # deal with the camera info
00123             infomsg = CameraInfo()
00124             infomsg.header = img.header
00125             # yes, this is only for an XTion / Kinect but that's the only thing supported by NAO
00126             ratio_x = float(640)/float(img.width)
00127             ratio_y = float(480)/float(img.height)
00128             infomsg.width = img.width
00129             infomsg.height = img.height
00130             # [ 525., 0., 3.1950000000000000e+02, 0., 525., 2.3950000000000000e+02, 0., 0., 1. ]
00131             infomsg.K = [ 525, 0, 3.1950000000000000e+02,
00132                          0, 525, 2.3950000000000000e+02,
00133                          0, 0, 1 ]
00134             infomsg.P = [ 525, 0, 3.1950000000000000e+02, 0,
00135                          0, 525, 2.3950000000000000e+02, 0,
00136                          0, 0, 1, 0 ]
00137 
00138             for i in range(3):
00139                 infomsg.K[i] = infomsg.K[i] / ratio_x
00140                 infomsg.K[3+i] = infomsg.K[3+i] / ratio_y
00141                 infomsg.P[i] = infomsg.P[i] / ratio_x
00142                 infomsg.P[4+i] = infomsg.P[4+i] / ratio_y
00143 
00144             infomsg.D = []
00145             infomsg.binning_x = 0
00146             infomsg.binning_y = 0
00147             infomsg.distortion_model = ""
00148             self.pub_info_.publish(infomsg)
00149 
00150             #Currently we only get depth image from the 3D camera of NAO, so we make up a fake color image (a black image)
00151             #and publish it under image_color topic.
00152             #This should be update when the color image from 3D camera becomes available.
00153             colorimg = np.zeros((img.height,img.width,3), np.uint8)
00154             try:
00155               cimg = self.bridge.cv2_to_imgmsg(colorimg, "bgr8")
00156               cimg.header.stamp = img.header.stamp
00157               cimg.header.frame_id = img.header.frame_id
00158               self.pub_cimg_.publish(cimg)
00159             except CvBridgeError, e:
00160               print e
00161 
00162             r.sleep()
00163 
00164 
00165         self.camProxy.unsubscribe(self.nameId)
00166 
00167 if __name__ == "__main__":
00168     try:
00169         naocam = NaoCam()
00170         naocam.main_loop()
00171     except RuntimeError as e:
00172         rospy.logerr('Something went wrong: %s' % str(e) )
00173     rospy.loginfo('Camera stopped')


romeo_sensors_py
Author(s): Ha Dang
autogenerated on Mon Apr 3 2017 03:27:36