Pr2LookAtFace.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import time
00004 
00005 import roslib
00006 roslib.load_manifest('actionlib')
00007 roslib.load_manifest('people_msgs')
00008 
00009 from actionlib import SimpleActionClient
00010 from face_detector.msg import *
00011 from pr2_controllers_msgs.msg import *
00012 
00013 from Action import Action
00014 from Pr2MoveHeadAction import Pr2MoveHeadAction
00015 
00016 import rospy
00017 
00018 class Pr2LookAtFace(Action):
00019 
00020     def __init__(self):
00021         super(Pr2LookAtFace, self).__init__()
00022         self._client = SimpleActionClient('face_detector_action',FaceDetectorAction)
00023         self._head_client = SimpleActionClient('/head_traj_controller/point_head_action', PointHeadAction)
00024         self._timer = None
00025         self._executing = False
00026         self._pending_face_goal = False
00027         self._pending_head_goal = False
00028 
00029     def set_duration(self, duration):
00030         duration = max(duration, 1.5)
00031         super(Pr2LookAtFace, self).set_duration(duration)
00032 
00033     def to_string(self):
00034         return 'look_at_face()'
00035 
00036     def execute(self):
00037         super(Pr2LookAtFace, self).execute()
00038         print('Pr2LookAtFace.execute() %d' % self.get_duration())
00039         # delay execution to not run nested in the current stack context
00040         self._timer = rospy.Timer(rospy.Duration.from_sec(0.001), self._execute, oneshot=True)
00041 
00042 
00043     def _execute(self, event):
00044         self._executing = True
00045         self._timer = rospy.Timer(rospy.Duration.from_sec(self.get_duration()), self._preempt, oneshot=True)
00046         hgoal = None
00047         connected = self._client.wait_for_server(rospy.Duration(1.0))
00048         if connected:
00049             while self._executing:
00050                 fgoal = FaceDetectorGoal()
00051                 self._pending_face_goal = True
00052                 self._client.send_goal(fgoal)
00053                 self._client.wait_for_result()
00054                 self._pending_face_goal = False
00055                 f = self._client.get_result()
00056                 if len(f.face_positions) > 0:
00057                     closest = -1
00058                     closest_dist = 1000
00059                     for i in range(len(f.face_positions)):
00060                         dist = f.face_positions[i].pos.x*f.face_positions[i].pos.x + f.face_positions[i].pos.y*f.face_positions[i].pos.y\
00061  + f.face_positions[i].pos.z*f.face_positions[i].pos.z
00062                         if dist < closest_dist:
00063                             closest = i
00064                             closest_dist = dist
00065                     if closest > -1:
00066                         hgoal = PointHeadGoal()
00067                         hgoal.target.header.frame_id = f.face_positions[closest].header.frame_id
00068                         hgoal.target.point.x = f.face_positions[closest].pos.x
00069                         hgoal.target.point.y = f.face_positions[closest].pos.y
00070                         hgoal.target.point.z = f.face_positions[closest].pos.z
00071                         hgoal.min_duration = rospy.Duration(1.0)        
00072                         if self._executing:
00073                             hconnected = self._head_client.wait_for_server(rospy.Duration(1.0))
00074                             if hconnected and self._executing:
00075                                 self._pending_head_goal = True
00076                                 self._head_client.send_goal(hgoal)
00077                                 #                            self._head_client.wait_for_result()
00078                                 #                            self._pending_head_goal = False
00079 
00080         else:
00081             hgoal = PointHeadGoal()
00082             hgoal.target.header.frame_id = "base_footprint";
00083             hgoal.target.point.x = 2.0
00084             hgoal.target.point.y = -2.0
00085             hgoal.target.point.z = 1.2
00086             hgoal.min_duration = rospy.Duration(1.0)
00087             if self._executing:
00088                 hconnected = self._head_client.wait_for_server(rospy.Duration(1.0))
00089                 if hconnected and self._executing:
00090                     self._pending_head_goal = True
00091                     self._head_client.send_goal(hgoal)
00092                     self._head_client.wait_for_result()
00093                     self._pending_head_goal = False
00094                     if self._executing:
00095                         time.sleep(1.0)
00096         
00097         self._finished_finding_face()
00098 
00099 
00100     def _preempt(self, event):
00101         self._executing = False
00102         if self._pending_face_goal:
00103             self._client.cancel_goal()
00104         if self._pending_head_goal:
00105             self._head_client.cancel_goal()
00106         self._execute_finished()
00107 
00108     def _finished_finding_face(self):
00109         if self._executing:
00110             self._executing = False
00111             self._timer.shutdown()
00112             self._execute_finished()


slider_gui
Author(s): Dirk Thomas
autogenerated on Wed Aug 26 2015 15:37:50