Go to the documentation of this file.00001 import roslib; roslib.load_manifest('face_reinterpret')
00002 from sensor_msgs.msg import PointCloud
00003 from visualization_msgs.msg import Marker
00004 import rospy
00005
00006 class FaceBox:
00007 def __init__(self):
00008 rospy.Subscriber('/face_detector/faces_cloud', PointCloud, self.callback)
00009 self.box_pub = rospy.Publisher('/face_detector/faces_boxes', Marker)
00010
00011 def callback(self, msg):
00012 if len(msg.points) <= 0:
00013 return
00014 m = Marker()
00015 m.header = msg.header
00016 m.id = 0
00017 m.type = Marker.CUBE
00018 m.action = Marker.ADD
00019 m.pose.position.x = msg.points[0].x
00020 m.pose.position.y = msg.points[0].y
00021 m.pose.position.z = msg.points[0].z
00022 m.scale.x = 0.2
00023 m.scale.y = 0.2
00024 m.scale.z = 0.2
00025 m.color.r = 1.0
00026 m.color.g = 0.0
00027 m.color.b = 0.0
00028 m.color.a = 0.5
00029 m.lifetime = rospy.Duration(10.0)
00030 print m
00031 self.box_pub.publish(m)
00032
00033
00034 if __name__ == '__main__':
00035 rospy.init_node('face_box')
00036 f = FaceBox()
00037 while not rospy.is_shutdown():
00038 rospy.sleep(1.0)