Go to the documentation of this file.00001 import rcommander.tool_utils as tu
00002 from PyQt4.QtGui import *
00003 from PyQt4.QtCore import *
00004 import numpy as np
00005 import smach
00006 import rospy
00007 from tf_broadcast_server.srv import BroadcastTransform, GetTransforms, ClearTransforms
00008 from face_detector.msg import *
00009 import actionlib
00010 import geometry_msgs.msg as geo
00011 import rcommander.tf_utils as tfu
00012 from object_manipulator.convert_functions import mat_to_pose, stamp_pose, change_pose_stamped_frame
00013
00014 class FaceDetectTool(tu.ToolBase):
00015
00016 def __init__(self, rcommander):
00017 tu.ToolBase.__init__(self, rcommander, 'face_detect', 'Detect Face', FaceDetectState)
00018 self.frames_service = rospy.ServiceProxy('get_transforms', GetTransforms)
00019 self.clear_frames_service = rospy.ServiceProxy('clear_all_transforms', ClearTransforms)
00020 self.default_frame = '/base_link'
00021 self.tf_listener = rcommander.tf_listener
00022
00023 def fill_property_box(self, pbox):
00024 formlayout = pbox.layout()
00025
00026
00027
00028 self.time_out_box = QDoubleSpinBox(pbox)
00029 self.time_out_box.setMinimum(1.)
00030 self.time_out_box.setMaximum(1000.)
00031 self.time_out_box.setSingleStep(1.)
00032
00033 self.frame_box = tu.FrameBox(self.frames_service)
00034 formlayout.addRow("&Orientation Frame", self.frame_box.create_box(pbox))
00035 formlayout.addRow('&Time Out', self.time_out_box)
00036 self.reset()
00037
00038 def new_node(self, name=None):
00039 if name == None:
00040 nname = self.name + str(self.counter)
00041 else:
00042 nname = name
00043
00044 ori_frame = self.frame_box.text()
00045 time_out = self.time_out_box.value()
00046 return FaceDetectState(nname, ori_frame, time_out)
00047
00048 def reset(self):
00049 self.frame_box.set_text(self.default_frame)
00050 self.time_out_box.setValue(120.)
00051
00052 def set_node_properties(self, node):
00053 self.frame_box.set_text(node.orientation_frame)
00054 self.time_out_box.setValue(node.time_out)
00055
00056
00057 class FaceDetectState(tu.StateBase):
00058
00059 def __init__(self, name, orientation_frame, time_out):
00060 tu.StateBase.__init__(self, name)
00061 self.orientation_frame = orientation_frame
00062 self.time_out = time_out
00063
00064 def get_smach_state(self):
00065 return FaceDetectFaceSmach(self.get_name(), self.orientation_frame, self.time_out)
00066
00067
00068 class FaceDetectFaceSmach(tu.ActionWithTimeoutSmach):
00069
00070 def __init__(self, frame_name, orientation_frame, time_out):
00071 tu.ActionWithTimeoutSmach.__init__(self, time_out, 'face_detector_action', FaceDetectorAction)
00072 self.broadcast_transform_srv = rospy.ServiceProxy('broadcast_transform', BroadcastTransform)
00073 self.orientation_frame = orientation_frame
00074 self.frame_name = frame_name
00075
00076 def get_goal(self):
00077 return FaceDetectorGoal()
00078
00079 def set_robot(self, robot):
00080 self.robot = robot
00081
00082 def process_result(self, face_result):
00083 face = face_result.face_positions[0]
00084 point_stamped = geo.PointStamped()
00085 point_stamped.header.frame_id = face.header.frame_id
00086 point_stamped.point = face.pos
00087 frame_id_T_frame_name = tfu.origin_to_frame(point_stamped, self.orientation_frame,
00088 self.robot.tf_listener, point_stamped.header.frame_id)
00089 ps = stamp_pose(mat_to_pose(frame_id_T_frame_name), point_stamped.header.frame_id)
00090 ps = change_pose_stamped_frame(self.robot.tf_listener, ps, '/base_link')
00091 self.broadcast_transform_srv(self.frame_name, ps)
00092
00093
00094
00095
00096
00097
00098