| 17 | |
| 18 | |
| 19 | class DeliverOrobotNode: |
| 20 | def __init__(self): |
| 21 | self.names = {} |
| 22 | |
| 23 | self.target_name = 'Kenji Koide' |
| 24 | self.names_sub = rospy.Subscriber('/face_recognition/people_names', NameArray, self.names_callback, queue_size=1, buff_size=2**10) |
| 25 | self.tracks_sub = rospy.Subscriber('/face_recognition/people_tracks', TrackArray, self.tracks_callback, queue_size=1, buff_size=2**10) |
| 26 | |
| 27 | self.done = False |
| 28 | self.move_base_action = actionlib.SimpleActionClient('move_base', MoveBaseAction) |
| 29 | while not self.move_base_action.wait_for_server(rospy.Duration(5)): |
| 30 | print 'waiting for the action server' |
| 31 | |
| 32 | def tracks_callback(self, tracks_msg): |
| 33 | if self.done: |
| 34 | return |
| 35 | |
| 36 | for track in tracks_msg.tracks: |
| 37 | face_id = track.id |
| 38 | |
| 39 | if face_id in self.names and self.names[face_id] == self.target_name: |
| 40 | pos = (track.x, track.y, 0.0) |
| 41 | print 'target found!!', pos |
| 42 | self.move_to(pos) |
| 43 | self.done = True |
| 44 | |
| 45 | def names_callback(self, names_msg): |
| 46 | names = {} |
| 47 | for i in range(len(names_msg.ids)): |
| 48 | names[names_msg.ids[i]] = names_msg.names[i] |
| 49 | self.names = names |
| 50 | |
| 51 | def move_to(self, pos): |
| 52 | goal = MoveBaseGoal() |
| 53 | goal_pose = goal.target_pose |
| 54 | goal_pose.header.frame_id = '/world' |
| 55 | goal_pose.header.stamp = rospy.Time.now() |
| 56 | goal_pose.pose.position.x = pos[0] |
| 57 | goal_pose.pose.position.y = pos[1] |
| 58 | goal_pose.pose.position.z = 0.0 |
| 59 | |
| 60 | goal_pose.pose.orientation.x = 0.0 |
| 61 | goal_pose.pose.orientation.y = 0.0 |
| 62 | goal_pose.pose.orientation.z = 0.0 |
| 63 | goal_pose.pose.orientation.w = 1.0 |
| 64 | |
| 65 | self.move_base_action.send_goal_and_wait(goal) |
| 66 | |
| 67 | |
| 68 | def main(): |