human_sensing_states.py
Go to the documentation of this file.
00001 import roslib
00002 roslib.load_manifest('srs_states')
00003 import rospy
00004 import smach
00005 import smach_ros
00006 
00007 from math import *
00008 import copy
00009 from struct import *
00010 import tf
00011 
00012 from simple_script_server import *
00013 sss = simple_script_server()
00014 
00015 
00016 #from srs_grasping.srv import *
00017 from geometry_msgs.msg import *
00018 from cob_people_detection.srv import *
00019 from cob_people_detection_msgs.msg import *
00020 from srs_human_sensing.srv import *
00021 from srs_human_sensing.msg import *
00022 
00023 from srs_leg_detector.srv import *
00024 from srs_body_detector.srv import *
00025 
00026 
00027 from simple_script_server import *
00028 sss = simple_script_server()
00029 
00030 class leg_detection(smach.State):
00031     def __init__(self,object_name = ""):
00032         smach.State.__init__(
00033             self,
00034             outcomes=['succeeded','failed','preempted','retry'],
00035             input_keys=['sm_input'],
00036             output_keys=['pose_list','id','humans_pose','person_label'])
00037         
00038         self.srv_name_leg_detection = '/detect_legs'
00039         self.pose_list=[]
00040 
00041     def execute(self, userdata):
00042         rospy.loginfo("Leg detection ready.")
00043 
00044         if(userdata.sm_input.label==""):
00045                     rospy.loginfo("Searching for all peoples.")
00046         else:
00047                     rospy.loginfo("Searching for special person.")
00048 
00049         if self.preempt_requested():
00050            self.service_preempt()
00051            return 'preempted'
00052        
00053        
00054         try:
00055             rospy.wait_for_service(self.srv_name_leg_detection,10)
00056         except rospy.ROSException, e:
00057             print "Service not available: %s"%e
00058             return 'failed'
00059 
00060         # call leg detection service
00061         try:
00062             detector_service = rospy.ServiceProxy(self.srv_name_leg_detection, DetectLegs)
00063             req = DetectLegsRequest()
00064             res = detector_service(req)
00065             lenp=len(res.leg_list.points)
00066             if lenp > 3:
00067                 lenp=3
00068             if lenp==0:
00069                 return 'retry'   
00070             for i in range(0, lenp):    
00071                             
00072                  p=Pose2D()
00073                  p.x=res.leg_list.points[i].x
00074                  p.y=res.leg_list.points[i].y
00075                  p.theta=0
00076                  self.pose_list.append(p)
00077                  
00078             for i in range (0,len(userdata.sm_input.pose_furniture)):
00079                  p=Pose2D()
00080                  p.x=userdata.sm_input.pose_furniture[i].x
00081                  p.y=userdata.sm_input.pose_furniture[i].y
00082                  p.theta=userdata.sm_input.pose_furniture[i].theta
00083                  self.pose_list.append(p)
00084 
00085                  
00086             #print self.pose_list     
00087             if len(self.pose_list)==0:
00088                 return 'failed'   
00089               
00090             userdata.pose_list=self.pose_list
00091             print len(self.pose_list)
00092             userdata.id=0
00093             userdata.humans_pose=detect_human_array()
00094             userdata.person_label=userdata.sm_input.label
00095             return 'succeeded'
00096         
00097         except rospy.ServiceException, e:
00098             print "Service call failed: %s"%e
00099 
00100             return 'failed'
00101 
00102 
00103 class move_to_better_position(smach.State):
00104     def __init__(self):
00105         smach.State.__init__(self, 
00106                              outcomes=['succeeded', 'failed', 'preempted'],
00107                              input_keys = ['pose_list', 'id','humans_pose','person_label'],
00108                              output_keys=['pose_list_out','id_out','humans_pose_out','person_label_out'])
00109         
00110         self.target_object_name=''
00111         #self.object_list
00112         
00113         
00114         self.object_pose=Pose()
00115         self.bbpose=Pose()
00116         self.srv_name_people_detection='/cob_people_detection/detect_people'
00117         
00118     def execute(self, userdata):
00119         print userdata.id
00120         if self.preempt_requested():
00121            self.service_preempt()
00122            return 'preempted'
00123        
00124         
00125         #move to better position
00126         
00127         #get the robot's current pose from tf
00128         rb_pose = Pose2D()
00129         listener = tf.TransformListener()
00130         listener.waitForTransform("/map", "/base_link", rospy.Time(0), rospy.Duration(4.0)) #wait for 4secs for the coordinate to be transformed
00131         (trans,rot) = listener.lookupTransform("/map", "/base_link", rospy.Time(0))
00132         rb_pose.x = trans[0]
00133         rb_pose.y = trans[1]
00134         rb_pose_rpy = tf.transformations.euler_from_quaternion(rot)
00135         rb_pose.theta = rb_pose_rpy[2]
00136         rospy.sleep(0.5)
00137         
00138         x=rb_pose.x-userdata.pose_list[userdata.id].x
00139         y=rb_pose.y-userdata.pose_list[userdata.id].y
00140         
00141         
00142         #not sure about the direction
00143         theta=tan(x/y)       
00144          
00145         sss.move("base",[rb_pose.x,rb_pose.y,theta])
00146         
00147         userdata.id_out=userdata.id
00148         userdata.pose_list_out=userdata.pose_list
00149         userdata.humans_pose_out=userdata.humans_pose
00150         userdata.person_label_out=userdata.person_label
00151         
00152         return 'succeeded'
00153          
00154          
00155          
00156    
00157 class face_detection(smach.State):
00158   def __init__(self):
00159       smach.State.__init__(self, 
00160                            outcomes=['succeeded', 'failed', 'preempted','retry'],
00161                            input_keys = ['pose_list', 'id','humans_pose','person_label'],
00162                            output_keys=['pose_list_output','id_out','face_list','humans_pose_out','person_label_out'])
00163       
00164       self.srv_name_face_detection='/cob_people_detection/detect_people'    
00165         
00166       self.object_pose=Pose()
00167       self.bbpose=Pose()
00168       
00169   def execute(self, userdata):
00170         
00171 
00172         if self.preempt_requested():
00173            self.service_preempt()
00174            return 'preempted'
00175          
00176         #searching for all people 
00177         if (userdata.person_label==''):
00178  
00179             try:
00180                 rospy.wait_for_service(self.srv_name_face_detection,10)
00181             except rospy.ROSException, e:
00182                 print "Service not available: %s"%e
00183                 return 'next'
00184     
00185             # call object detection service
00186             try:
00187                 detector_service = rospy.ServiceProxy(self.srv_name_face_detection, DetectPeople)
00188                 req = DetectPeopleRequest()
00189                 res = detector_service(req)
00190                 if len(res.people_list.detections)==0:
00191                    userdata.id_out=userdata.id+1
00192                    userdata.pose_list_output=userdata.pose_list
00193                    return 'succeeded'    
00194                  
00195                 userdata.face_list=res.people_list
00196                 userdata.id_out=userdata.id
00197                 userdata.pose_list_output=userdata.pose_list
00198                 userdata.humans_pose_out=userdata.humans_pose
00199                 userdata.person_label_out=userdata.person_label
00200     
00201                 return 'succeeded'
00202                 
00203     
00204             except rospy.ServiceException, e:
00205                 print "Service call failed: %s"%e
00206     
00207                 return 'failed'
00208             
00209         #searching for a special label    
00210         try:
00211             rospy.wait_for_service(self.srv_name_face_detection,10)
00212         except rospy.ROSException, e:
00213             print "Service not available: %s"%e
00214             return 'succeeded'
00215 
00216         # call object detection service
00217         try:
00218             detector_service = rospy.ServiceProxy(self.srv_name_face_detection, DetectPeople)
00219             req = DetectPeopleRequest()
00220             res = detector_service(req)
00221     
00222             for i in range (0,len(res.people_list.detections)):
00223                 if(res.people_list.detections[i].label==userdata.person_label):
00224 
00225                     userdata.face_list=res.people_list.detections[i]
00226                     userdata.id_out=userdata.id
00227                     userdata.pose_list_output=userdata.pose_list
00228                     userdata.humans_pose_out=userdata.humans_pose
00229                     userdata.person_label_out=userdata.person_label
00230                     return 'succeeded'
00231 
00232             if userdata.id+1>len(userdata.pose_list)-1:
00233               print 'cant find special person'  
00234               return 'failed'  
00235             userdata.id_out=userdata.id+1
00236             userdata.pose_list_output=userdata.pose_list      
00237             return 'retry'
00238 
00239         except rospy.ServiceException, e:
00240             print "Service call failed: %s"%e
00241 
00242             return 'failed'
00243 class body_detection(smach.State):
00244   def __init__(self):
00245       smach.State.__init__(self, 
00246                            outcomes=['succeeded', 'failed', 'preempted'],
00247                            input_keys = ['pose_list', 'id','humans_pose','person_label','face_list'],
00248                            output_keys=['pose_list_output','id_out','bodies_list','humans_pose_out','face_list_out','person_label_out'])
00249       
00250       self.srv_name_body_detection='/detect_bodies'    
00251         
00252       self.object_pose=Pose()
00253       self.bbpose=Pose()
00254       
00255   def execute(self, userdata):
00256         print userdata.id
00257         if self.preempt_requested():
00258            self.service_preempt()
00259            return 'preempted'
00260          
00261          
00262          
00263          
00264         try:
00265             rospy.wait_for_service(self.srv_name_body_detection,10)
00266         except rospy.ROSException, e:
00267             print "Service not available: %s"%e
00268             return 'failed'
00269 
00270         # call object detection service
00271         try:
00272             body_detector_service = rospy.ServiceProxy(self.srv_name_body_detection, getBodyDetections)
00273             req = getBodyDetectionsRequest()
00274             res = body_detector_service(req)
00275    
00276             userdata.bodies_list=res.bodies_list
00277             userdata.id_out=userdata.id
00278             userdata.pose_list_output=userdata.pose_list
00279             userdata.humans_pose_out=userdata.humans_pose
00280             
00281             userdata.face_list_out=userdata.face_list
00282             userdata.person_label_out=userdata.person_label
00283 
00284             return 'succeeded'
00285             
00286 
00287         except rospy.ServiceException, e:
00288             print "Service call failed: %s"%e
00289 
00290             return 'failed'
00291                 
00292         
00293 class compare_detections(smach.State):
00294   def __init__(self):
00295       smach.State.__init__(self, 
00296                            outcomes=['succeeded', 'retry',  'failed', 'preempted'],
00297                            input_keys = ['pose_list','face_list','bodies_list', 'id','humans_pose','person_label'],
00298                            output_keys=['pose_list_out','id_out','humans_pose_out','person_label'])
00299       
00300       self.srv_name_compare='compare_hs_detections'
00301       self.human=[]
00302       self.body_list=[]
00303       self.face_list=[]
00304   def execute(self, userdata):
00305       
00306       print userdata.id
00307       if self.preempt_requested():
00308            self.service_preempt()
00309            return 'preempted'
00310              
00311       if (userdata.person_label==''):
00312 
00313           self.human_array=userdata.humans_pose 
00314 
00315              
00316           try:
00317                 rospy.wait_for_service(self.srv_name_compare,10)
00318           except rospy.ROSException, e:
00319                 print "Service not available: %s"%e
00320                 return 'failed'
00321         
00322         # call object detection service
00323           try:
00324                 detector_service = rospy.ServiceProxy(self.srv_name_compare, Comp_HS_Detections)
00325                 comp=Comp_HS_DetectionsRequest()
00326                 for i in range(len(userdata.face_list.detections)):
00327                     pose=Pose()
00328                     pose.position.x=userdata.face_list.detections[i].pose.pose.position.x
00329                     pose.position.z=userdata.face_list.detections[i].pose.pose.position.z
00330                     pose.position.y=userdata.face_list.detections[i].pose.pose.position.y
00331                     pose.orientation.x=userdata.face_list.detections[i].pose.pose.orientation.x
00332                     pose.orientation.z=userdata.face_list.detections[i].pose.pose.orientation.z
00333                     pose.orientation.y=userdata.face_list.detections[i].pose.pose.orientation.y
00334                     self.face_list.append(pose)
00335                     
00336                 comp.face_det=self.face_list
00337                 comp.leg_det=userdata.pose_list[userdata.id]
00338                    
00339                        
00340                 for i in range(len(userdata.bodies_list)):
00341                     pose=Pose()
00342                     pose.position.x=userdata.bodies_list[i].position.x
00343                     pose.position.z=userdata.bodies_list[i].position.z
00344                     pose.position.y=userdata.bodies_list[i].position.y
00345                     pose.orientation.x=userdata.bodies_list[i].orientation.x
00346                     pose.orientation.z=userdata.bodies_list[i].orientation.z
00347                     pose.orientation.y=userdata.bodies_list[i].orientation.y
00348                     self.body_list.append(pose)
00349                     
00350                 comp.face_det=self.body_list
00351                 comp.label=userdata.person_label
00352                 res = detector_service(comp)
00353                 for i in range(len(res.detect_human_array.detect_human)):
00354                     self.human_array.detect_human.append(res.detect_human_array.detect_human[i])
00355                                  
00356           
00357           except rospy.ServiceException, e:
00358                 print "Service call failed: %s"%e
00359     
00360                 return 'failed'
00361             
00362           userdata.humans_pose_out=self.human_array
00363           if userdata.id+1>len(userdata.pose_list)-1:
00364               print self.human_array
00365               return 'succeeded'
00366           userdata.id_out=userdata.id+1
00367           userdata.pose_list_out=userdata.pose_list
00368           userdata.person_label_out=userdata.person_label
00369       
00370           return 'retry'
00371       
00372       
00373       
00374       #compare label
00375       
00376       
00377 
00378       try:
00379             rospy.wait_for_service(self.srv_name_compare,10)
00380       except rospy.ROSException, e:
00381             print "Service not available: %s"%e
00382             return 'failed'
00383     
00384         # call object detection service
00385       try:
00386             compare_service = rospy.ServiceProxy(self.srv_name_compare, Comp_HS_Detections)
00387             comp=Comp_HS_DetectionsRequest()
00388            
00389 
00390                 
00391             comp.leg_det=userdata.pose_list[userdata.id]
00392             
00393             
00394             pose=Pose()
00395             pose=userdata.face_list.pose.pose
00396             self.face_list.append(pose)
00397                     
00398             for i in range(len(userdata.bodies_list)):
00399                     pose=Pose()
00400                     pose.position.x=userdata.bodies_list[i].position.x
00401                     pose.position.z=userdata.bodies_list[i].position.z
00402                     pose.position.y=userdata.bodies_list[i].position.y
00403                     pose.orientation.x=userdata.bodies_list[i].orientation.x
00404                     pose.orientation.z=userdata.bodies_list[i].orientation.z
00405                     pose.orientation.y=userdata.bodies_list[i].orientation.y
00406                     self.body_list.append(pose)
00407             comp.label=userdata.person_label
00408             comp.face_det=self.face_list
00409             comp.body_det=self.body_list
00410 
00411             res = compare_service(comp)
00412             print res
00413             userdata.humans_pose_out=res
00414 
00415                     
00416             
00417                             
00418                     
00419       
00420       except rospy.ServiceException, e:
00421             print "Service call failed: %s"%e
00422 
00423             return 'failed'
00424         
00425       print self.human    
00426       return 'succeeded'
00427 
00428       


srs_human_sensing
Author(s): Alex
autogenerated on Sun Jan 5 2014 12:21:20