nao_vision: nao_vision.py Source File

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 #
00004 # ROS node to serve NAOqi Vision functionalities
00005 # This code is currently compatible to NaoQI version 1.6
00006 #
00007 # Copyright 2014 Manos Tsardoulias, CERTH/ITI
00008 # http://www.ros.org/wiki/nao
00009 #
00010 # Redistribution and use in source and binary forms, with or without
00011 # modification, are permitted provided that the following conditions are met:
00012 #
00013 #    # Redistributions of source code must retain the above copyright
00014 #       notice, this list of conditions and the following disclaimer.
00015 #    # Redistributions in binary form must reproduce the above copyright
00016 #       notice, this list of conditions and the following disclaimer in the
00017 #       documentation and/or other materials provided with the distribution.
00018 #    # Neither the name of the CERTH/ITI nor the names of its
00019 #       contributors may be used to endorse or promote products derived from
00020 #       this software without specific prior written permission.
00021 #
00022 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00023 # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00024 # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00025 # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00026 # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00027 # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00028 # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00029 # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00030 # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00031 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 # POSSIBILITY OF SUCH DAMAGE.
00033 #
00034 
00035 import rospy
00036 import naoqi
00037 
00038 from naoqi import ( 
00039     ALModule, 
00040     ALBroker, 
00041     ALProxy)
00042     
00043 from nao_driver import NaoNode
00044 
00045 #~ ROS msgs
00046 from std_msgs.msg import (
00047     String, 
00048     Float32, 
00049     Int32)
00050     
00051 from std_srvs.srv import (
00052     EmptyResponse,
00053     Empty)
00054     
00055 from geometry_msgs.msg import (
00056     Point)
00057 
00058 #~ nao-ros msgs
00059 from nao_interaction_msgs.msg import (
00060     FaceDetected,
00061     MovementDetected,
00062     LandmarkDetected)
00063     
00064 from nao_interaction_msgs.srv import (
00065     VisionMotionSensitivity)
00066 
00067 class Constants:
00068     NODE_NAME = "nao_vision_interface"
00069 
00070 class NaoVisionInterface(ALModule, NaoNode):
00071     "sss"
00072     def __init__(self, moduleName):
00073         # ROS initialization
00074         NaoNode.__init__(self)
00075         rospy.init_node( Constants.NODE_NAME )
00076         
00077         # NAOQi initialization
00078         self.ip = ""
00079         self.port = 10601
00080         self.moduleName = moduleName
00081         self.init_almodule()
00082         
00083         #~ Variable initialization
00084         self.faces = FaceDetected()  
00085         face_detection_enabled = False
00086         motion_detection_enabled = False
00087         landmark_detection_enabled = False
00088              
00089         #~ ROS initializations
00090         self.subscribeFaceSrv = rospy.Service("nao_vision/face_detection/enable", Empty, self.serveSubscribeFaceSrv)
00091         self.unsubscribeFaceSrv = rospy.Service("nao_vision/face_detection/disable", Empty, self.serveUnsubscribeFaceSrv)
00092         self.subscribeMotionSrv = rospy.Service("nao_vision/motion_detection/enable", Empty, self.serveSubscribeMotionSrv)
00093         self.unsubscribeMotionSrv = rospy.Service("nao_vision/motion_detection/disable", Empty, self.serveUnsubscribeMotionSrv)
00094         self.subscribeLandmarkSrv = rospy.Service("nao_vision/landmark_detection/enable", Empty, self.serveSubscribeLandmarkSrv)
00095         self.unsubscribeLandmarkSrv = rospy.Service("nao_vision/landmark_detection/disable", Empty, self.serveUnsubscribeLandmarkSrv)
00096         
00097         self.subscribe()
00098         
00099         rospy.loginfo("nao_vision_interface initialized")
00100 
00101     def init_almodule(self):
00102         # before we can instantiate an ALModule, an ALBroker has to be created
00103         rospy.loginfo("Connecting to NaoQi at %s:%d", self.pip, self.pport)
00104         try:
00105             self.broker = ALBroker("%sBroker" % self.moduleName, self.ip, self.port, self.pip, self.pport)
00106         except RuntimeError,e:
00107             print("Could not connect to NaoQi's main broker")
00108             exit(1)
00109         ALModule.__init__(self, self.moduleName)
00110         
00111         self.memProxy = ALProxy("ALMemory",self.pip,self.pport)
00112         if self.memProxy is None:
00113             rospy.logerror("Could not get a proxy to ALMemory on %s:%d", self.pip, self.pport)
00114             exit(1)
00115             
00116         self.landmarkDetectionProxy = ALProxy("ALLandMarkDetection",self.pip,self.pport)
00117         if self.landmarkDetectionProxy is None:
00118             rospy.logerror("Could not get a proxy to ALLandMarkDetection on %s:%d", self.pip, self.pport)
00119             exit(1)
00120         
00121         self.movementDetectionProxy = ALProxy("ALMovementDetection",self.pip,self.pport)
00122         if self.movementDetectionProxy is None:
00123             rospy.logerror("Could not get a proxy to ALMovementDetection on %s:%d", self.pip, self.pport)
00124             exit(1)
00125 
00126     def shutdown(self): 
00127         self.unsubscribe()
00128 
00129     def subscribe(self):
00130         #~ Subscriptions are performed via srv calls to save cpu power 
00131         pass
00132 
00133     def unsubscribe(self):
00134         if landmark_detection_enabled:
00135             self.memProxy.unsubscribeToEvent("LandmarkDetected", self.moduleName)
00136         if face_detection_enabled:
00137             self.memProxy.unsubscribeToEvent("FaceDetected", self.moduleName)
00138         if motion_detection_enabled:
00139             self.memProxy.unsubscribeToEvent("MovementDetection/MovementDetected", self.moduleName)
00140         
00141 
00142     def onLandmarkDetected(self, strVarName, value, strMessage):
00143         "Called when landmark was detected"
00144         if len(value) == 0:
00145             return
00146             
00147         # For the specific fields in the value variable check here:
00148         # https://community.aldebaran-robotics.com/doc/1-14/naoqi/vision/allandmarkdetection-api.html#landmarkdetected-value-structure
00149         
00150         msg = LandmarkDetected()  
00151         
00152         for i in range (0, len(value[1])):
00153             msg.shape_alpha.append(Float32(value[1][i][0][1]))
00154             msg.shape_beta.append(Float32(value[1][i][0][2]))
00155             msg.shape_sizex.append(Float32(value[1][i][0][3]))
00156             msg.shape_sizey.append(Float32(value[1][i][0][4]))
00157             msg.mark_ids.append(Int32(value[1][i][1][0]))
00158             
00159         msg.camera_local_pose.position.x = value[2][0]
00160         msg.camera_local_pose.position.y = value[2][1]
00161         msg.camera_local_pose.position.z = value[2][2]
00162         msg.camera_local_pose.orientation.x = value[2][3]
00163         msg.camera_local_pose.orientation.y = value[2][4]
00164         msg.camera_local_pose.orientation.z = value[2][5]
00165         
00166         msg.camera_world_pose.position.x = value[3][0]
00167         msg.camera_world_pose.position.y = value[3][1]
00168         msg.camera_world_pose.position.z = value[3][2]
00169         msg.camera_world_pose.orientation.x = value[3][3]
00170         msg.camera_world_pose.orientation.y = value[3][4]
00171         msg.camera_world_pose.orientation.z = value[3][5]
00172         
00173         msg.camera_name = String(value[4])
00174         
00175         self.landmarkPub.publish(msg)
00176 
00177     def onFaceDetected(self, strVarName, value, strMessage):
00178         "Called when a face was detected"
00179         
00180         if len(value) == 0:
00181           return
00182         
00183         # For the specific fields in the value variable check here:
00184         #https://community.aldebaran-robotics.com/doc/1-14/naoqi/vision/alfacedetection.html
00185         
00186         self.faces.camera_id.data = int(value[4]);
00187         
00188         self.faces.camera_0_pose.position.x = float(value[2][0])
00189         self.faces.camera_0_pose.position.y = float(value[2][1])
00190         self.faces.camera_0_pose.position.z = float(value[2][2])
00191         self.faces.camera_0_pose.orientation.x = float(value[2][3])
00192         self.faces.camera_0_pose.orientation.y = float(value[2][4])
00193         self.faces.camera_0_pose.orientation.z = float(value[2][5])
00194         
00195         self.faces.camera_1_pose.position.x = float(value[3][0])
00196         self.faces.camera_1_pose.position.y = float(value[3][1])
00197         self.faces.camera_1_pose.position.z = float(value[3][2])
00198         self.faces.camera_1_pose.orientation.x = float(value[3][3])
00199         self.faces.camera_1_pose.orientation.y = float(value[3][4])
00200         self.faces.camera_1_pose.orientation.z = float(value[3][5])
00201         
00202         self.faces.shape_alpha.data = float(value[1][0][0][1])
00203         self.faces.shape_beta.data = float(value[1][0][0][2])
00204         self.faces.shape_sizeX.data = float(value[1][0][0][3])
00205         self.faces.shape_sizeY.data = float(value[1][0][0][4])
00206         
00207         self.faces.face_id.data = float(value[1][0][1][0])
00208         self.faces.score_reco.data = float(value[1][0][1][1])
00209         self.faces.face_label.data = str(value[1][0][1][2])
00210         
00211         self.faces.left_eye_eyeCenter_x.data = float(value[1][0][1][3][0])
00212         self.faces.left_eye_eyeCenter_y.data = float(value[1][0][1][3][1])
00213         self.faces.left_eye_noseSideLimit_x.data = float(value[1][0][1][3][2])
00214         self.faces.left_eye_noseSideLimit_y.data = float(value[1][0][1][3][3])
00215         self.faces.left_eye_earSideLimit_x.data = float(value[1][0][1][3][4])
00216         self.faces.left_eye_earSideLimit_y.data = float(value[1][0][1][3][5])
00217         self.faces.left_eye_topLimit_x.data = float(value[1][0][1][3][6])
00218         self.faces.left_eye_topLimit_y.data = float(value[1][0][1][3][7])
00219         self.faces.left_eye_bottomLimit_x.data = float(value[1][0][1][3][8])
00220         self.faces.left_eye_bottomLimit_y.data = float(value[1][0][1][3][9])
00221         self.faces.left_eye_midTopEarLimit_x.data = float(value[1][0][1][3][10])
00222         self.faces.left_eye_midTopEarLimit_y.data = float(value[1][0][1][3][11])
00223         self.faces.left_eye_midTopNoseLimit_x.data = float(value[1][0][1][3][12])
00224         self.faces.left_eye_midTopNoseLimit_y.data = float(value[1][0][1][3][13])
00225         
00226         self.faces.right_eye_eyeCenter_x.data = float(value[1][0][1][4][0])
00227         self.faces.right_eye_eyeCenter_y.data = float(value[1][0][1][4][1])
00228         self.faces.right_eye_noseSideLimit_x.data = float(value[1][0][1][4][2])
00229         self.faces.right_eye_noseSideLimit_y.data = float(value[1][0][1][4][3])
00230         self.faces.right_eye_earSideLimit_x.data = float(value[1][0][1][4][4])
00231         self.faces.right_eye_earSideLimit_y.data = float(value[1][0][1][4][5])
00232         self.faces.right_eye_topLimit_x.data = float(value[1][0][1][4][6])
00233         self.faces.right_eye_topLimit_y.data = float(value[1][0][1][4][7])
00234         self.faces.right_eye_bottomLimit_x.data = float(value[1][0][1][4][8])
00235         self.faces.right_eye_bottomLimit_y.data = float(value[1][0][1][4][9])
00236         self.faces.right_eye_midTopEarLimit_x.data = float(value[1][0][1][4][10])
00237         self.faces.right_eye_midTopEarLimit_y.data = float(value[1][0][1][4][11])
00238         self.faces.right_eye_midTopNoseLimit_x.data = float(value[1][0][1][4][12])
00239         self.faces.right_eye_midTopNoseLimit_y.data = float(value[1][0][1][4][13])
00240         
00241         self.faces.left_eyebrow_noseSideLimit_x.data = float(value[1][0][1][5][0])
00242         self.faces.left_eyebrow_noseSideLimit_y.data = float(value[1][0][1][5][1])
00243         self.faces.left_eyebrow_center_x.data = float(value[1][0][1][5][2])
00244         self.faces.left_eyebrow_center_y.data = float(value[1][0][1][5][3])
00245         self.faces.left_eyebrow_earSideLimit_x.data = float(value[1][0][1][5][4])
00246         self.faces.left_eyebrow_earSideLimit_y.data = float(value[1][0][1][5][5])
00247         
00248         self.faces.right_eyebrow_noseSideLimit_x.data = float(value[1][0][1][6][0])
00249         self.faces.right_eyebrow_noseSideLimit_y.data = float(value[1][0][1][6][1])
00250         self.faces.right_eyebrow_center_x.data = float(value[1][0][1][6][2])
00251         self.faces.right_eyebrow_center_y.data = float(value[1][0][1][6][3])
00252         self.faces.right_eyebrow_earSideLimit_x.data = float(value[1][0][1][6][4])
00253         self.faces.right_eyebrow_earSideLimit_y.data = float(value[1][0][1][6][5])
00254         
00255         self.faces.nose_bottomCenterLimit_x.data = float(value[1][0][1][7][0])
00256         self.faces.nose_bottomCenterLimit_y.data = float(value[1][0][1][7][1])
00257         self.faces.nose_bottomLeftLimit_x.data = float(value[1][0][1][7][2])
00258         self.faces.nose_bottomLeftLimit_y.data = float(value[1][0][1][7][3])
00259         self.faces.nose_bottomRightLimit_x.data = float(value[1][0][1][7][4])
00260         self.faces.nose_bottomRightLimit_y.data = float(value[1][0][1][7][5])
00261         
00262         self.faces.mouth_leftLimit_x.data = float(value[1][0][1][8][0])
00263         self.faces.mouth_leftLimit_y.data = float(value[1][0][1][8][1])
00264         self.faces.mouth_rightLimit_x.data = float(value[1][0][1][8][2])
00265         self.faces.mouth_rightLimit_y.data = float(value[1][0][1][8][3])
00266         self.faces.mouth_topLimit_x.data = float(value[1][0][1][8][4])
00267         self.faces.mouth_topLimit_y.data = float(value[1][0][1][8][5])
00268         self.faces.mouth_bottomLimit_x.data = float(value[1][0][1][8][6])
00269         self.faces.mouth_bottomLimit_y.data = float(value[1][0][1][8][7])
00270         self.faces.mouth_midTopLeftLimit_x.data = float(value[1][0][1][8][8])
00271         self.faces.mouth_midTopLeftLimit_y.data = float(value[1][0][1][8][9])
00272         self.faces.mouth_midTopRightLimit_x.data = float(value[1][0][1][8][10])
00273         self.faces.mouth_midTopRightLimit_y.data = float(value[1][0][1][8][11])
00274         self.faces.mouth_midBottomRightLimit_x.data = float(value[1][0][1][8][12])
00275         self.faces.mouth_midBottomRightLimit_y.data = float(value[1][0][1][8][13])
00276         self.faces.mouth_midBottomLeftLimit_x.data = float(value[1][0][1][8][14])
00277         self.faces.mouth_midBottomLeftLimit_y.data = float(value[1][0][1][8][15])
00278 
00279         self.facesPub.publish(self.faces)
00280 
00281     def handleSensitivityChangeSrv(self, req):
00282         if (req.sensitivity.data < 0.0) or (req.sensitivity.data > 1.0):
00283             return
00284         
00285         self.movementDetectionProxy.setSensitivity(req.sensitivity.data)
00286         rospy.loginfo("Sensitivity of nao_movement_detection changed to %f", req.sensitivity.data)
00287 
00288     def onMovementDetected(self, strVarName, value, strMessage):
00289         "Called when movement was detected"
00290         datavar = self.memProxy.getData("MovementDetection/MovementInfo")
00291         
00292         movement = MovementDetected()  
00293         
00294         movement.gravity_center.x = datavar[0][0][0]
00295         movement.gravity_center.y = datavar[0][0][1]
00296 
00297         movement.mean_velocity.x = datavar[0][1][0]
00298         movement.mean_velocity.y = datavar[0][1][1]
00299 
00300         for i in range(0, len(datavar[0][2])):
00301           movement.points_poses.append(Point())
00302           movement.points_poses[i].x = datavar[0][2][i][0]
00303           movement.points_poses[i].y = datavar[0][2][i][1]
00304           
00305           movement.points_speeds.append(Point())
00306           movement.points_speeds[i].x = datavar[0][3][i][0]
00307           movement.points_speeds[i].y = datavar[0][3][i][1]
00308         
00309         self.movementPub.publish(movement)
00310 
00311     def serveSubscribeFaceSrv(self, req):
00312         self.facesPub = rospy.Publisher("nao_vision/faces_detected", FaceDetected)
00313         self.memProxy.subscribeToEvent("FaceDetected", self.moduleName, "onFaceDetected")
00314         self.face_detection_enabled = True
00315         
00316     def serveUnsubscribeFaceSrv(self, req):
00317         if self.face_detection_enabled:
00318             self.memProxy.unsubscribeToEvent("FaceDetected", self.moduleName)
00319             self.facesPub.unregister()
00320             self.face_detection_enabled = False
00321         
00322     def serveSubscribeMotionSrv(self, req):
00323         self.movementPub = rospy.Publisher("nao_vision/movement_detected", MovementDetected)
00324         self.sensitivitySrv = rospy.Service("nao_vision/movement_detection_sensitivity", VisionMotionSensitivity, self.handleSensitivityChangeSrv )
00325         self.memProxy.subscribeToEvent("MovementDetection/MovementDetected", self.moduleName, "onMovementDetected")
00326         self.motion_detection_enabled = True
00327         
00328     def serveUnsubscribeMotionSrv(self, req):
00329         if self.motion_detection_enabled:
00330             self.memProxy.unsubscribeToEvent("MovementDetection/MovementDetected", self.moduleName)
00331             self.movementPub.unregister()
00332             self.sensitivitySrv.shutdown()
00333             self.motion_detection_enabled = False
00334         
00335     def serveSubscribeLandmarkSrv(self, req):
00336         self.landmarkPub = rospy.Publisher("nao_vision/landmark_detected", LandmarkDetected)
00337         self.memProxy.subscribeToEvent("LandmarkDetected", self.moduleName, "onLandmarkDetected")
00338         self.landmarkDetectionProxy.updatePeriod("ROSNaoVisionModuleLandmarkDetected", 200)
00339         self.landmark_detection_enabled = True
00340         
00341     def serveUnsubscribeLandmarkSrv(self, req):
00342         if self.landmark_detection_enabled:
00343             self.memProxy.unsubscribeToEvent("LandmarkDetected", self.moduleName)
00344             self.landmarkPub.unregister()
00345             self.landmark_detection_enabled = False
00346         
00347 if __name__ == '__main__':
00348   
00349     ROSNaoVisionModule = NaoVisionInterface("ROSNaoVisionModule")
00350     rospy.spin()
00351 
00352     rospy.loginfo("Stopping ROSNaoVisionModule ...")
00353     ROSNaoVisionModule.shutdown();        
00354     rospy.loginfo("ROSNaoVisionModule stopped.")
00355     exit(0)