challenge_hand_me_that
get_furniture_from_operator_pose.py
Go to the documentation of this file.
1 #
2 # Copyright (c) 2019, TU/e Robotics, Netherlands
3 # All rights reserved.
4 #
5 # \author Rein Appeldoorn
6 
7 import os
8 
9 import rospy
10 
11 from ed.entity import Entity
12 from geometry_msgs.msg import PoseStamped
13 from robot_skills import get_robot, robot
14 from robot_smach_states.util.designators import is_writeable, VariableDesignator
15 from robot_smach_states.utility import CheckTimeOut, WriteDesignator
16 from smach import StateMachine, cb_interface, CBState
17 from std_msgs.msg import Header
18 from people_recognition_msgs.msg import Person3D
19 
20 
21 OPERATOR = None # type: Person3D
22 
23 # For Testing
24 # INDEX = 0
25 # OPERATORS = [Person3D(), Person3D()]
26 # OPERATOR1 = OPERATORS[0]
27 # OPERATOR1.header.frame_id = "map"
28 # OPERATOR1.position.z = 3
29 # OPERATOR1.pointing_pose.position.x = 3.5245514495275265
30 # OPERATOR1.pointing_pose.position.y = 7.01266033925749
31 # OPERATOR1.pointing_pose.position.z = 1.1662874869084487
32 # OPERATOR1.pointing_pose.orientation.x = -0.44843423312166364
33 # OPERATOR1.pointing_pose.orientation.y = 0.3778370454325741
34 # OPERATOR1.pointing_pose.orientation.z = -0.16792463126864846
35 # OPERATOR1.pointing_pose.orientation.w = 0.7924312108168484
36 # OPERATOR2 = OPERATORS[1]
37 # OPERATOR2.header.frame_id = "map"
38 # OPERATOR2.position.z = 3
39 # OPERATOR2.pointing_pose.position.x = 2.0
40 # OPERATOR2.pointing_pose.position.y = 8.8
41 # OPERATOR2.pointing_pose.position.z = 1.2
42 # OPERATOR2.pointing_pose.orientation.x = -0.27059805007309845
43 # OPERATOR2.pointing_pose.orientation.y = -0.6532814824381882
44 # OPERATOR2.pointing_pose.orientation.z = 0.6532814824381883
45 # OPERATOR2.pointing_pose.orientation.w = -0.27059805007309845
46 
47 
48 class GetFurnitureFromOperatorPose(StateMachine):
49  def __init__(self, robot, furniture_designator, possible_furniture):
50  # type: (robot.Robot, VariableDesignator) -> None
51  StateMachine.__init__(self, outcomes=['done', 'failed'], output_keys=["laser_dot"])
52 
53  # For Testing
54  # self.transform_pub = TransformBroadcaster()
55 
56  is_writeable(furniture_designator)
57  reset_des = VariableDesignator(resolve_type=bool).writeable
58 
59  def _show_view(timeout=5):
60  rgb, depth, depth_info = robot.perception.get_rgb_depth_caminfo()
61  robot.hmi.show_image_from_msg(rgb, timeout)
62  return rgb, depth, depth_info
63 
64  @cb_interface(outcomes=['done'])
65  def _prepare_operator(_):
66  global OPERATOR
67  OPERATOR = None
68 
69  # For Testing
70  # return 'done'
71 
72  robot.ed.reset()
73  robot.head.reset()
74  robot.speech.speak("Let's point, please stand in front of me!", block=False)
75  for _ in range(6):
76  _show_view(timeout=2)
77  rospy.sleep(0.4)
78 
79  _show_view(timeout=1)
80  robot.speech.speak("Please point at the object you want me to hand you", block=False) # hmm, weird sentence
81  rospy.sleep(0.4)
82  for _ in range(2):
83  _show_view(timeout=2)
84  rospy.sleep(0.4)
85 
86  _show_view(timeout=1)
87  robot.speech.speak("Three")
88  _show_view(timeout=1)
89  robot.speech.speak("Two")
90  _show_view(timeout=1)
91  robot.speech.speak("One")
92 
93  return 'done'
94 
95  @cb_interface(outcomes=['done', 'failed'])
96  def _get_operator(_):
97  global OPERATOR
98 
99  def _is_operator(person):
100  if person.position.z > 3.5:
101  robot.speech.speak("Please stand CLOSER to me, so I can see your full body")
102  return False
103 
104  if person.position.z < 1.5:
105  robot.speech.speak("Please stand FURTHER away from me, so I can see your full body")
106  return False
107 
108  if "is_pointing" not in person.tags:
109  robot.speech.speak("Please point with your arm stretched")
110  return False
111 
112  return True
113 
114  # # Shortcut people recognition; For testing
115  # global INDEX
116  # OPERATOR = OPERATORS[INDEX]
117  # INDEX = (INDEX+1) % 2
118  #
119  # transform = TransformStamped()
120  # transform.header = OPERATOR.header
121  # transform.header.stamp = rospy.Time.now()
122  # transform.child_frame_id = "bla"
123  # transform.transform.rotation = OPERATOR.pointing_pose.orientation
124  # transform.transform.translation.x = OPERATOR.pointing_pose.position.x
125  # transform.transform.translation.y = OPERATOR.pointing_pose.position.y
126  # transform.transform.translation.z = OPERATOR.pointing_pose.position.z
127  # self.transform_pub.sendTransform(transform)
128 
129  start = rospy.Time.now()
130  while not rospy.is_shutdown() and OPERATOR is None and (rospy.Time.now() - start).to_sec() < 10:
131  persons = robot.perception.detect_person_3d(*_show_view())
132  if persons:
133  persons = sorted(persons, key=lambda x: x.position.z)
134  person = persons[0]
135  if _is_operator(person):
136  OPERATOR = person
137  break
138  else:
139  rospy.logerr("Could not find an correct operator. Let him point again")
140  OPERATOR = None
141  return "failed"
142 
143  # robot.speech.speak("I see an operator at %.2f meter in front of me" % OPERATOR.position.z)
144  rospy.loginfo("I see an operator at %.2f meter in front of me" % OPERATOR.position.z)
145 
146  return 'done'
147 
148  @cb_interface(outcomes=['done', 'failed'], output_keys=['laser_dot'])
149  def _get_furniture(user_data):
150  global OPERATOR
151 
152  final_result = None
153  while not rospy.is_shutdown() and final_result is None:
154  result = None
155  attempts = 0
156  while not rospy.is_shutdown() and attempts < 5:
157  try:
158  attempts += 1
159  map_pose = robot.tf_buffer.transform(PoseStamped(
160  header=Header(
161  frame_id=OPERATOR.header.frame_id,
162  stamp=rospy.Time.now() - rospy.Duration.from_sec(0.5)
163  ),
164  pose=OPERATOR.pointing_pose
165  ), "map")
166  rospy.logwarn(f"{map_pose=}")
167  result = robot.ed.ray_trace(map_pose)
168  if result is not None:
169  break
170  # For testing
171  # result = RayTraceResponse()
172  # result.entity_id = "desk"
173  # result.intersection_point.header.frame_id = "map"
174  # result.intersection_point.point.x = 3.73
175  # result.intersection_point.point.y = 6.05
176  # result.intersection_point.point.z = 0.75
177  except Exception as e:
178  rospy.logerr("Could not get ray trace from closest person: {}".format(e))
179  rospy.sleep(0.5)
180  else:
181  rospy.logerr("Could not find an entity with the current operator pose. Let him point again")
182  OPERATOR = None
183  return "failed"
184 
185 
186  # result.intersection_point type: PointStamped
187  # result.entity_id: string
188  rospy.loginfo("There is a ray intersection with {i} at ({p.x:.4}, {p.y:.4}, {p.z:.4})".format(i=result.entity_id, p=result.intersection_point.point))
189 
190  if result.entity_id in possible_furniture:
191  final_result = result
192  else:
193  rospy.loginfo("{} is not furniture".format(result.entity_id))
194  robot.speech.speak("You pointed at: {}. That's not furniture".format(result.entity_id))
195  rospy.sleep(1)
196  OPERATOR = None
197  robot.get_arm(required_goals=["reset"]).send_joint_goal("reset")
198  return 'failed'
199 
200  robot.speech.speak("You pointed at %s" % final_result.entity_id)
201  robot.get_arm(required_goals=["reset"]).send_joint_goal("reset")
202  # Fill the designator and user data the furniture inspection
203  furniture_designator.write(robot.ed.get_entity(final_result.entity_id))
204  user_data['laser_dot'] = result.intersection_point
205  rospy.loginfo(f"{result=}")
206  return 'done'
207 
208  with self:
209  self.add("WRITE_RESET_DES_TRUE", WriteDesignator(reset_des, True),
210  transitions={'written': 'PREPARE_OPERATOR'})
211  self.add('PREPARE_OPERATOR', CBState(_prepare_operator), transitions={'done': 'CHECK_TIMEOUT'})
212  self.add("CHECK_TIMEOUT", CheckTimeOut(30, reset_des), transitions={'not_yet': 'GET_OPERATOR',
213  'time_out': 'failed'})
214  self.add('GET_OPERATOR', CBState(_get_operator), transitions={'done': 'GET_FURNITURE',
215  'failed': 'CHECK_TIMEOUT'})
216  self.add('GET_FURNITURE', CBState(_get_furniture), transitions={'done': 'done', 'failed': 'CHECK_TIMEOUT'})
217 
218 
219 if __name__ == '__main__':
220  from robocup_knowledge import load_knowledge
221  rospy.init_node(os.path.splitext("test_" + os.path.basename(__file__))[0])
222  furniture_designator_ = VariableDesignator(resolve_type=Entity)
223  robot_instance = get_robot("hero")
224  robot_instance.reset()
225  knowledge = load_knowledge("challenge_hand_me_that")
226  sm = GetFurnitureFromOperatorPose(robot_instance, furniture_designator_.writeable, knowledge.all_possible_furniture)
227  while not rospy.is_shutdown():
228  sm.execute()
challenge_hand_me_that.get_furniture_from_operator_pose.GetFurnitureFromOperatorPose.__init__
def __init__(self, robot, furniture_designator, possible_furniture)
Definition: get_furniture_from_operator_pose.py:49
challenge_hand_me_that.get_furniture_from_operator_pose.GetFurnitureFromOperatorPose
Definition: get_furniture_from_operator_pose.py:48
Generated on Tue Apr 15 2025 04:37:24 for challenge_hand_me_that by   doxygen 1.8.17