hs.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 import roslib; roslib.load_manifest('srs_human_sensing')
00003 import rospy
00004 import math
00005 from std_msgs.msg import String
00006 from sensor_msgs.msg import PointCloud
00007 from geometry_msgs.msg import Point32
00008 from cob_people_detection_msgs.msg import PeopleDetectionArray
00009 from srs_human_sensing.srv import Comp_HS_Detections
00010 import tf
00011 from std_msgs.msg import Header
00012 from srs_human_sensing.msg import *
00013 
00014 
00015 def callback_LD(data):
00016     rospy.loginfo("I heard from LD:")
00017     leg_detections = data.points
00018     global legs 
00019     legs = []
00020     for i in range (len(leg_detections)):
00021        legs.append(leg_detections[i])
00022        print leg_detections[i].x,",",leg_detections[i].y
00023     
00024     match_detections()
00025 
00026 
00027 def callback_FD(data):
00028     rospy.loginfo("I heard from FD:")
00029     face_detections = data.detections
00030     
00031     
00032     global faces 
00033     faces = []
00034 
00035     for i in range (len(face_detections)):
00036        
00037     
00038 
00039        try:
00040 
00041           detection1 = tflistener.transformPose('map', face_detections[i].pose)        
00042           newpoint = Point32(detection1.pose.position.x,detection1.pose.position.y,detection1.pose.position.z)
00043           faces.append(newpoint)
00044 
00045        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): 
00046          print "tf transform error"
00047    
00048     match_detections () 
00049 
00050 def match_detections ():
00051     distances = [] 
00052     global legs
00053     global faces
00054     global pub
00055 
00056     pc = PointCloud()
00057     pc.header = Header(0,rospy.get_rostime(),'/map')
00058    
00059      
00060     
00061     
00062 
00063     for m in range (len(faces)):
00064                 pc.points.append(Point32(faces[m].x,faces[m].y,1.5))
00065    
00066     for l in range (len(legs)):
00067          pc.points.append(Point32(legs[l].x,legs[l].y,1.0))
00068          distancecol = []
00069          for m in range (len(faces)):
00070                 
00071            # measure the distance between the detections and store them
00072                 dist = (sqrt((faces[m].x-legs[l].x)**2+(faces[m].y-legs[l].y)**2))
00073                 distancecol.append (dist)
00074                  
00075          distances.append (distancecol) 
00076          
00077     print "distances"
00078     print distances
00079     
00080      
00081     
00082     
00083    
00084     
00085     pub.publish (pc)
00086       
00087 def handle_compare_hs_detections (req):
00088     dh=detect_human()
00089     dha=detect_human_array()
00090 
00091     dh.pose.x=1
00092     dh.pose.y=2
00093     dh.probability=80
00094     dha.detect_human.append(dh)
00095     return dha
00096 
00097 def listener():
00098     
00099     rospy.Subscriber("leg_detections_cloud", PointCloud , callback_LD)
00100     rospy.Subscriber("/cob_people_detection/face_position_array", PeopleDetectionArray , callback_FD)
00101     rospy.spin()
00102 
00103 if __name__ == '__main__':
00104     print "Listening ..."
00105 
00106 
00107     rospy.init_node('SRS_HS_listener', anonymous=True)
00108     legs = []
00109     faces = []
00110     tflistener = tf.TransformListener()
00111     tflistener.waitForTransform("/head_cam3d_link", "/map", rospy.Time(), rospy.Duration(5.0))
00112     pub = rospy.Publisher('people_detections_cloud', PointCloud)
00113     serv = rospy.Service ('compare_hs_detections',Comp_HS_Detections,handle_compare_hs_detections)
00114     
00115     listener()


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