def execute(self, userdata=None): entity = self.grab_entity_designator.resolve() if not entity: rospy.logerr("Could not resolve grab_entity") return "failed" self.robot.head.look_at_point(VectorStamped(vector=entity._pose.p, frame_id="/map"), timeout=0.0) self.robot.head.wait_for_motion_done() segm_res = self.robot.ed.update_kinect("%s" % entity.id) arm = self.arm_designator.resolve() if not arm: rospy.logerr("Could not resolve arm") return "failed" # Torso up (non-blocking) self.robot.torso.reset() # Arm to position in a safe way arm.send_joint_trajectory('prepare_grasp', timeout=0) arm.wait_for_motion_done() # Open gripper arm.send_gripper_goal('open', timeout=0.0) arm.wait_for_motion_done() # Make sure the head looks at the entity self.robot.head.look_at_point(VectorStamped(vector=entity._pose.p, frame_id="/map"), timeout=0.0) self.robot.head.wait_for_motion_done() return 'succeeded'
def get_closest_entity(self, type="", center_point=None, radius=0): if not center_point: center_point = VectorStamped(x=0, y=0, z=0, frame_id="/" + self.robot_name + "/base_link") entities = self.get_entities(type=type, center_point=center_point, radius=radius) # HACK entities = [ e for e in entities if e.shape is not None and e.type != "" ] if len(entities) == 0: return None # Sort by distance try: center_in_map = center_point.projectToFrame( "/map", self._tf_listener) entities = sorted( entities, key=lambda entity: entity.distance_to_2d(center_in_map.vector)) except: rospy.logerr("Failed to sort entities") return None return entities[0]
def execute(self, userdata): grab_entity = self.grab_entity_designator.resolve() if not grab_entity: rospy.logerr("Could not resolve grab_entity") return "failed" arm = self.arm_designator.resolve() if not arm: rospy.logerr("Could not resolve arm") return "failed" userdata.arm = arm.side # Using userdata makes sure we don't need to do any more arm entity magic goal_map = VectorStamped(0, 0, 0, frame_id=grab_entity.id) try: # Transform to base link frame goal_bl = goal_map.projectToFrame( self.robot.robot_name + '/base_link', tf_listener=self.robot.tf_listener) if goal_bl is None: rospy.logerr('Transformation of goal to base failed') return 'failed' except tf.Exception, tfe: rospy.logerr( 'Transformation of goal to base failed: {0}'.format(tfe)) return 'failed'
def execute(self, userdata=None): # Get position to look at. Transform the position to map frame since taking the hmi pose may move base link goal = VectorStamped(1.0, 0.0, 1.6, frame_id="/" + self._robot.robot_name + "/base_link") tf_goal = goal.projectToFrame('/map', self._robot.tf_listener) self._robot.move_to_hmi_pose() self._robot.head.look_at_point(tf_goal) self._robot.head.wait_for_motion_done() return "succeeded"
def _get_head_goal(self, spec): vs = VectorStamped(spec["x"], spec["y"], z=0, frame_id="/" + self._robot.robot_name + "/base_link") return vs
def _recognize(self): z = 1.5 self.robot.head.look_at_point( VectorStamped(100, 0, z, self.robot.robot_name + "/base_link")) self.robot.speech.speak("I am looking for my operator", block=False) # 1) Check how many people in the crowd shots = 3 number_of_people = 0 operator_list = [] sentences = [ "You are all looking great today! Keep looking in my camera!", "I like it when everybody is staring at me; being in the center of attention!" ] for i in range(0, shots): self.robot.speech.speak(sentences[i % (shots - 1)], block=False) detections, operator = self._get_detections( external_api_request=False) # Get number of people number_of_people = max(len(detections), number_of_people) # Get operator guess if operator: operator_list.append(operator) # 2) Get all other information with use of the external api, make sure that we have all persons here max_tries = 5 try_number = 0 best_operator = None best_detections = None while True: try_number += 1 self.robot.speech.speak( random.choice([ "Let's take a closer look", "Let's see what we are dealing with", "Let's get some more details" ])) detections, operator = self._get_detections( external_api_request=True) if self._shot_valid(number_of_people, operator_list, detections, operator): return detections, operator if not best_detections or len(detections) > len(best_detections): best_operator = operator best_detections = detections if try_number > max_tries: self.robot.speech.speak( "I am having trouble with my external servers, I will use my local result" ) # we just didnt pass our criteria return best_detections, best_operator
def get_closest_laser_entity(self, type="", center_point=VectorStamped(), radius=0): """ Get the closest entity detected by the laser. The ID's of such entities are postfixed with '-laser' For the rest, this works exactly like get_closest_entity :param type: What type of entities to filter on :param center_point: combined with radius. Around which point to search for entities :param radius: how far from the center_point to look (in meters) :return: list of Entity """ entities = self.get_entities(type="", center_point=center_point, radius=radius) # HACK entities = [ e for e in entities if e.shape and e.type == "" and e.id.endswith("-laser") ] if len(entities) == 0: return None # Sort by distance try: entities = sorted(entities, key=lambda entity: entity.distance_to_2d(center_point.projectToFrame("/%s/base_link"%self.robot_name, self._tf_listener).vector)) # TODO: adjust for robot except: print "Failed to sort entities" return None return entities[0]
def run(self, robot, entity, area, waittime): if not entity: return 'failed' # Entities define their own frame, so there is no need to transform the pose to /map. # That would be equivalent to defining coordinates 0,0,0 in its own frame, so that is what we do here. # The added benefit is that the entity's frame actually moves because the entity is tracked. # This makes the head track the entity frame_id = "/" + entity.id if area in entity.volumes: cp = entity.volumes[area].center_point vs = VectorStamped(cp.x(), cp.y(), cp.z(), frame_id) rospy.loginfo('Look at %s' % (repr(vs))) # This is awefully hardcoded for AMIGO!!! (TODO) height = min(0.4, max(0.1, vs.vector.z() - 0.55)) robot.torso._send_goal([height], timeout=0) robot.head.look_at_point(vs, timeout=0) robot.head.wait_for_motion_done(timeout=5) robot.torso.wait_for_motion_done(timeout=5) rospy.sleep(rospy.Duration(waittime)) return "succeeded" rospy.logwarn("Cannot find {0} in {1}".format(area, entity.id)) return "failed"
def get_entities(self, type="", center_point=VectorStamped(), radius=0, id="", ignore_z=False): center_point_in_map = center_point.projectToFrame( "/map", self.tf_listener) entities = self._entities.values() if type: entities = [e for e in entities if e.is_a(type)] if radius: if ignore_z: entities = [ e for e in entities if e.distance_to_2d(center_point_in_map.vector) <= radius ] else: entities = [ e for e in entities if e.distance_to_3d(center_point_in_map.vector) <= radius ] if id: entities = [e for e in entities if e.id == id] return entities
def get_entities(self, type="", center_point=VectorStamped(), radius=0, id="", parse=True): self._publish_marker(center_point, radius) center_point_in_map = center_point.projectToFrame( "/map", self._tf_listener) query = SimpleQueryRequest(id=id, type=type, center_point=kdl_vector_to_point_msg( center_point_in_map.vector), radius=radius) try: entity_infos = self._ed_simple_query_srv(query).entities entities = map(from_entity_info, entity_infos) except Exception, e: rospy.logerr( "ERROR: robot.ed.get_entities(id=%s, type=%s, center_point=%s, radius=%s)" % (id, type, str(center_point), str(radius))) rospy.logerr("L____> [%s]" % e) return []
def _get_detections(self, external_api_request): z = 1.5 self.robot.head.look_at_point( VectorStamped(100, 0, z, self.robot.robot_name + "/base_link")) self.robot.head.wait_for_motion_done() time.sleep(1) rospy.logerr( "ed.detect _persons() method disappeared! This was only calling the face recognition module and we are using a new one now!" ) rospy.logerr("I will return an empty detection list!") detections = [] if not detections: detections = [] operator_candidates = [ candidate for candidate in detections if candidate.name == "operator" ] rospy.loginfo("Detections: %s", detections) rospy.loginfo("Operator candidates: %s", operator_candidates) operator = None if operator_candidates: operator = max(operator_candidates, key=lambda c: c.name_score) rospy.loginfo("Operator: %s", operator) return detections, operator
def get_entities(self, type="", center_point=VectorStamped(), radius=float('inf'), id="", ignore_z=False): """ Get entities via Simple Query interface :param type: Type of entity :param center_point: Point from which radius is measured :param radius: Distance between center_point and entity :param id: ID of entity :param ignore_z: Consider only the distance in the X,Y plane for the radius from center_point """ self._publish_marker(center_point, radius) center_point_in_map = center_point.projectToFrame("/map", self.tf_listener) query = SimpleQueryRequest(id=id, type=type, center_point=kdl_vector_to_point_msg(center_point_in_map.vector), radius=radius, ignore_z=ignore_z) try: entity_infos = self._ed_simple_query_srv(query).entities entities = map(from_entity_info, entity_infos) except Exception as e: rospy.logerr("ERROR: robot.ed.get_entities(id={}, type={}, center_point={}, radius={}, ignore_z={})".format( id, type, str(center_point), str(radius), ignore_z)) rospy.logerr("L____> [%s]" % e) return [] return entities
def get_closest_possible_person_entity(self, center_point=VectorStamped(), radius=float('inf')): """ Returns the 'possible_human' entity closest to a certain center point. :param center_point: (VectorStamped) indicating where the human should be close to :param radius: (float) radius to look for possible humans :return: (Entity) entity (if found), None otherwise """ assert center_point.frame_id.endswith("map"), "Other frame ids not yet implemented" # Get all entities entities = self.get_entities(type="", center_point=center_point, radius=radius) # Filter on 'possible humans' entities = [e for e in entities if e.is_a('possible_human')] # If nothing found, return None if len(entities) == 0: return None # Sort by distance try: entities = sorted(entities, key=lambda entity: entity.distance_to_2d(center_point.vector)) rospy.logdebug("entities sorted closest to robot = {}".format(entities)) except Exception as e: rospy.logerr("Failed to sort entities: {}".format(e)) return None return entities[0]
def get_closest_laser_entity(self, type="", center_point=VectorStamped(), radius=float('inf'), ignore_z=False): """ Get the closest entity detected by the laser. The ID's of such entities are postfixed with '-laser' For the rest, this works exactly like get_closest_entity :param type: What type of entities to filter on :param center_point: combined with radius. Around which point to search for entities :param radius: how far from the center_point to look (in meters) :param ignore_z: Consider only the distance in the X,Y plane for the radius from center_point criterium. :return: list of Entity """ entities = self.get_entities(type="", center_point=center_point, radius=radius, ignore_z=ignore_z) # HACK entities = [e for e in entities if e.shape and e.type == "" and e.id.endswith("-laser")] if len(entities) == 0: return None # Sort by distance try: entities = sorted(entities, key=lambda entity: entity.distance_to_2d( center_point.projectToFrame("/%s/base_link" % self.robot_name, self.tf_listener).vector)) # TODO: adjust for robot except Exception as e: rospy.logerr("Failed to sort entities: {}".format(e)) return None return entities[0]
def get_closest_possible_person_entity(self, type="", center_point=VectorStamped(), radius=0, room=""): #if isinstance(center_point, PointStamped): # center_point = self._transform_center_point_to_map(center_point) # ToDo: check frame ids entities = self.get_entities(type="", center_point=center_point, radius=radius) # HACK # entities = [ e for e in entities if e.convex_hull and e.type == "" and 'possible_human' in e.flags ] entities = [e for e in entities if e.is_a('possible_human')] if len(entities) == 0: return None # Sort by distance try: entities = sorted( entities, key=lambda entity: entity.distance_to_2d(center_point.vector)) print "entities sorted closest to robot = ", entities except: print "Failed to sort entities" return None return entities[0]
def execute(self, userdata): arm = self.arm_designator.resolve() if not arm: rospy.logerr("Could not resolve arm") return "failed" userdata.arm = arm.side entity = self.grab_entity_designator.resolve() if not entity: rospy.logerr("Could not resolve grab_entity") return "failed" # Open gripper (non-blocking) arm.send_gripper_goal('open', timeout=0) # Torso up (non-blocking) self.robot.torso.high() # Arm to position in a safe way arm.send_joint_trajectory('prepare_grasp', timeout=0) # Open gripper arm.send_gripper_goal('open', timeout=0.0) # Make sure the head looks at the entity self.robot.head.look_at_point(VectorStamped(vector=entity._pose.p, frame_id="/map"), timeout=0.0) return 'succeeded'
def __init__(self, robot, type="", center_point=None, radius=float('inf'), id="", criteriafuncs=None, weight_function=None, type_designator=None, center_point_designator=None, id_designator=None, debug=False, name=None): """Designates an entity of some type, within a radius of some center_point, with some id, that match some given criteria functions. @param robot the robot to use for Ed queries @param type the type of the entity to resolve to (default: any type) @param center_point combined with radius: a sphere to search an entity in @param radius combined with center_point: a sphere to search an entity in @param id the ID of the object to get info about @param criteriafuncs a list of functions that take an entity and return a bool (True if criterium met) @param weight_function returns a weight for each entity, the one with the lowest weight will be selected (could be a distance calculation) @param type_designator same as type but dynamically resolved trhough a designator. Mutually exclusive with type @param center_point_designator same as center_point but dynamically resolved trhough a designator. Mutually exclusive with center_point @param id_designator same as id but dynamically resolved through a designator. Mutually exclusive with id""" super(EdEntityDesignator, self).__init__(resolve_type=Entity, name=name) assert not type or type_designator is None, "Specify either type or type_designator, not both" assert center_point is None or center_point_designator is None, \ "Specify either center_point or center_point_designator, not both" assert not id or id_designator is None, "Specify either id or id_designator, not both" self.robot = robot self.ed = robot.ed self.type = type if center_point is None and center_point_designator is None: center_point = VectorStamped() self.center_point = center_point self.radius = radius self.id = id self.criteriafuncs = criteriafuncs or [] self.weight_function = weight_function or (lambda entity: 0) if type_designator: # the resolve type of type_designator can be either str or list check_resolve_type(type_designator, str, list) self.type_designator = type_designator if center_point_designator: # the resolve type of type_designator can be either VectorStamped check_resolve_type(center_point_designator, VectorStamped) self.center_point_designator = center_point_designator if id_designator: # the resolve type of id_designator must be str check_resolve_type(id_designator, str) self.id_designator = id_designator self.debug = debug
def execute(self, userdata=None): """ Does the actual work :param userdata: :return: """ self._robot.head.reset() rospy.sleep(1) self._robot.speech.speak("I'm waiting for a waving person") waving_persons = [] while not rospy.is_shutdown() and not waving_persons: rospy.sleep(1 / self.rate) for person in self.people_received.people: if {'RWave', 'LWave'}.intersection(set(person.tags)): waving_persons.append(person) if not waving_persons: return 'aborted' rospy.loginfo('waving persons: %s', waving_persons) if len(waving_persons) > 1: rospy.logwarn('using the first person') header = self.people_received.header point = waving_persons[0].position pose = frame_stamped(header.frame_id, point.x, point.y, point.z) rospy.loginfo('update customer position to %s', pose) self._robot.ed.update_entity(id=self._caller_id, frame_stamped=pose, type="waypoint") # customer_point_msg_stamped_camera = PointStamped() # customer_point_msg_stamped_camera = self.people_received.header # customer_point_msg_stamped_camera = waving_persons[0].position # self.robot.tf_listener. # look at the barman kitchen_entity = self._kitchen_designator.resolve() target_pose = kitchen_entity._pose head_target_kdl = target_pose * kdl.Vector(20.0, 0.0, 0.0) head_target = VectorStamped(x=head_target_kdl.x(), y=head_target_kdl.y(), z=head_target_kdl.z(), frame_id="/map") # pose = kitchen_entity.pose.extractVectorStamped() # pose.vector[2] = 1.5 # self._robot.head.look_at_point(head_target) self._robot.speech.speak( "I have seen a waving person, should I continue?") if self._confirm(): self._robot.head.cancel_goal() return 'succeeded' else: self._robot.head.cancel_goal() return 'rejected'
def execute(self): # convert item to pos and send goal goal_map = VectorStamped(0, 0, 0, frame_id=self.item.id) goal_bl = goal_map.projectToFrame(self.robot.robot_name + '/base_link', tf_listener=self.robot.tf_listener) if goal_bl is None: return 'failed' else: return 'failed' # Offset so goal_bl is in WS # tfToWSOffset = find ws from base_link and find the difference goal_bl.vector[0] = goal_bl.vector.x() - tfToWSOffset if not self.arm.send_goal(goal_bl, timeout=0): # maybe register callbacks when return ('failed') # movement finished? not waiting
def get_shot(self): z = 1.5 self.robot.head.look_at_point( VectorStamped(100, 0, z, self.robot.robot_name + "/base_link")) self.robot.head.wait_for_motion_done() time.sleep(1) image = self.robot.head.get_image() return self._bridge.imgmsg_to_cv2(image, 'bgr8')
def reset(self, timeout=0): """ Reset head position """ reset_goal = VectorStamped(x=10, frame_id="/" + self.robot_name + "/base_link") return self.look_at_point(reset_goal, timeout=timeout)
def __init__(self, robot, type="", center_point=None, radius=0, id="", parse=True, criteriafuncs=None, type_designator=None, center_point_designator=None, id_designator=None, debug=False, name=None): """Designates a collection of entities of some type, within a radius of some center_point, with some id, that match some given criteria functions. @param robot the robot to use for Ed queries @param type the type of the entity to resolve to (default: any type) @param center_point combined with radius: a sphere to search an entity in @param radius combined with center_point: a sphere to search an entity in @param id the ID of the object to get info about @param parse whether to parse the data string associated with the object model or entity @param type_designator same as type but dynamically resolved trhough a designator. Mutually exclusive with type @param center_point_designator same as center_point but dynamically resolved through a designator. Mutually exclusive with center_point @param id_designator same as id but dynamically resolved through a designator. Mutually exclusive with id""" super(EdEntityCollectionDesignator, self).__init__(resolve_type=[Entity], name=name) self.ed = robot.ed if type != "" and type_designator != None: raise TypeError("Specify either type or type_designator, not both") if center_point != None and center_point_designator != None: raise TypeError( "Specify either center_point or center_point_designator, not both" ) elif center_point == None and center_point_designator == None: center_point = VectorStamped() if id != "" and id_designator != None: raise TypeError("Specify either id or id_designator, not both") self.type = type self.center_point = center_point self.radius = radius self.id = id self.parse = parse self.criteriafuncs = criteriafuncs or [] if type_designator: check_resolve_type(type_designator, str) self.type_designator = type_designator if center_point_designator: check_resolve_type(center_point_designator, VectorStamped) self.center_point_designator = center_point_designator if id_designator: check_resolve_type(id_designator, str) self.id_designator = id_designator self.debug = debug
def look_up(self, timeout=0): """ Gives a target at z = 1.0 at 1 m in front of the robot """ goal = VectorStamped(0.2, 0.0, 4.5, frame_id="/" + self.robot_name + "/base_link") return self.look_at_point(goal, timeout=timeout)
def look_at_standing_person(self, timeout=0): """ Gives a target at z = 1.75 at 1 m in front of the robot """ goal = VectorStamped(1.0, 0.0, 1.6, frame_id="/" + self.robot_name + "/base_link") return self.look_at_point(goal, timeout=timeout)
def __init__(self, robot_name, tf_listener, *args, **kwargs): super(Perception, self).__init__(robot_name, tf_listener) self.reset = mock.MagicMock() self.close = mock.MagicMock() self.learn_person = mock.MagicMock() self.detect_faces = mock.MagicMock() self.get_best_face_recognition = mock.MagicMock() self.get_rgb_depth_caminfo = mock.MagicMock() self.project_roi = lambda *args, **kwargs: VectorStamped( random.random(), random.random(), random.random(), "/map")
def test_look_at_enity_looks_at_correct_point(self): """Test that the robot looks at the center point of the named area, w.r.t. the frame of the entity""" entity_ds = ds.Designator(self.entity) state = states.LookAtEntity(self.robot, entity_ds, waittime=0) state.execute() vs = VectorStamped(0, 0, 0, "/12345") self.robot.head.look_at_point.assert_called_with(vs)
def get_closest_room(self, center_point=None, radius=0): if not center_point: center_point = VectorStamped(x=0, y=0, z=0, frame_id="/" + self.robot_name + "/base_link") return self.get_closest_entity(type="room", center_point=center_point, radius=radius)
def project_roi(self, roi, frame_id=None): """ Projects a region of interest of a depth image to a 3D Point. Hereto, a service is used :param roi: sensor_msgs/RegionOfInterest :param frame_id: if specified, the result is transformed into this frame id :return: VectorStamped object """ response = self.project_rois(rois=[roi]).points[0] # Convert to VectorStamped result = VectorStamped(x=response.point.x, y=response.point.y, z=response.point.z, frame_id=response.header.frame_id) # If necessary, transform the point if frame_id is not None: rospy.loginfo("Transforming roi to {}".format(frame_id)) result = result.projectToFrame(frame_id=frame_id, tf_listener=self.tf_listener) # Return the result return result
def test_head(amigo): global joint_positions max_err = 0.02 # straight vs = VectorStamped(x=0.4, z=10, frame_id='/amigo/torso') amigo.head.look_at_point(vs) # print str(joint_positions['neck_pan_joint']) + " " + str(joint_positions['neck_tilt_joint']) show_test( "head straight", in_bounds(joint_positions['neck_pan_joint'], 0, max_err) and in_bounds(joint_positions['neck_tilt_joint'], 0, max_err)) # up vs = VectorStamped(x=0.5, z=1, frame_id='/amigo/torso') amigo.head.look_at_point(vs) # print str(joint_positions['neck_pan_joint']) + " " + str(joint_positions['neck_tilt_joint']) show_test( "head up", in_bounds(joint_positions['neck_pan_joint'], 0, max_err) and in_bounds(joint_positions['neck_tilt_joint'], -0.26, max_err)) # down vs = VectorStamped(x=-0.4, z=1, frame_id='/amigo/torso') amigo.head.look_at_point(vs) # print str(joint_positions['neck_pan_joint']) + " " + str(joint_positions['neck_tilt_joint']) show_test( "head down", in_bounds(joint_positions['neck_pan_joint'], 0, max_err) and in_bounds(joint_positions['neck_tilt_joint'], 0.54, max_err)) # right vs = VectorStamped(x=0.4, y=100, z=1, frame_id='/amigo/torso') amigo.head.look_at_point(vs) # print str(joint_positions['neck_pan_joint']) + " " + str(joint_positions['neck_tilt_joint']) show_test( "head right", in_bounds(joint_positions['neck_pan_joint'], -1.547, max_err) and in_bounds(joint_positions['neck_tilt_joint'], 0, max_err)) # left vs = VectorStamped(x=0.4, y=-100, z=1, frame_id='/amigo/torso') amigo.head.look_at_point(vs) # print str(joint_positions['neck_pan_joint']) + " " + str(joint_positions['neck_tilt_joint']) show_test( "head left", in_bounds(joint_positions['neck_pan_joint'], 1.547, max_err) and in_bounds(joint_positions['neck_tilt_joint'], 0, max_err)) # straight vs = VectorStamped(x=0.4, z=10, frame_id='/amigo/torso') amigo.head.look_at_point(vs) # print str(joint_positions['neck_pan_joint']) + " " + str(joint_positions['neck_tilt_joint']) show_test( "head straight", in_bounds(joint_positions['neck_pan_joint'], 0, max_err) and in_bounds(joint_positions['neck_tilt_joint'], 0, max_err))
def look_at_segmentation_area(robot, entity, volume=None): """ Has a robot look at a certain object and possibly a volume :param robot: robot object :param entity: entity to look at :param volume: string indicating the specific volume to look at (e.g., 'on_top_on' or 'shelf3') """ # Determine the height of the head target # Start with a default pos = entity.pose.frame.p look_at_point_z = 0.7 # Check if we have areas: use these if volume in entity.volumes: search_volume = entity.volumes[volume] try: look_at_point_z = search_volume.min_corner.z() except Exception as e: rospy.logerr( "Cannot get z_min of volume {} of entity {}: {}".format( volume, entity.id, e.message)) else: # Look at the top of the entity to inspect pos = entity.pose.frame.p try: look_at_point_z = pos.z + entity.shape.z_max except Exception as e: rospy.logerr("Cannot get z_max of entity {}: {}".format( entity.id, e.message)) # Point the head at the right direction robot.head.look_at_point(VectorStamped(pos.x(), pos.y(), look_at_point_z, "/map"), timeout=0) # Make sure the spindle is at the appropriate height if we are AMIGO if robot.robot_name == "amigo": # Send spindle goal to bring head to a suitable location # Correction for standard height: with a table heigt of 0.76 a spindle position # of 0.35 is desired, hence offset = 0.76-0.35 = 0.41 # Minimum: 0.15 (to avoid crushing the arms), maximum 0.4 spindle_target = max( 0.15, min(look_at_point_z - 0.41, robot.torso.upper_limit[0])) robot.torso._send_goal([spindle_target], timeout=0) robot.torso.wait_for_motion_done() robot.head.wait_for_motion_done()
def execute(self, userdata=None): grab_entity = self.grab_entity_designator.resolve() if not grab_entity: rospy.logerr("Could not resolve grab_entity") return "failed" arm = self.arm_designator.resolve() if not arm: rospy.logerr("Could not resolve arm") return "failed" goal_map = VectorStamped(0, 0, 0, frame_id=grab_entity.id) try: # Transform to base link frame goal_bl = goal_map.projectToFrame(self.robot.robot_name+'/base_link', tf_listener=self.robot.tf_listener) if goal_bl is None: rospy.logerr('Transformation of goal to base failed') return 'failed' except tf2_ros.TransformException as tfe: rospy.logerr('Transformation of goal to base failed: {0}'.format(tfe)) return 'failed' # Make sure the torso and the arm are done self.robot.torso.wait_for_motion_done(cancel=True) arm.wait_for_motion_done(cancel=True) # This is needed because the head is not entirely still when the # look_at_point function finishes rospy.sleep(rospy.Duration(0.5)) # Resolve the entity again because we want the latest pose updated_grab_entity = self.grab_entity_designator.resolve() rospy.loginfo("ID to update: {0}".format(grab_entity.id)) if not updated_grab_entity: rospy.logerr("Could not resolve the updated grab_entity, " "this should not happen [CHECK WHY THIS IS HAPPENING]") grab_entity = self.associate(original_entity=grab_entity) else: rospy.loginfo("Updated pose of entity (dx, dy, dz) : (%f, %f, %f)" % (updated_grab_entity.pose.frame.p.x() - grab_entity.pose.frame.p.x(), updated_grab_entity.pose.frame.p.y() - grab_entity.pose.frame.p.y(), updated_grab_entity.pose.frame.p.z() - grab_entity.pose.frame.p.z())) grab_entity = updated_grab_entity # - - - - - - - - - - - - - - - - - - - - - - - - - - - - # Grasp point determination grasp_framestamped = self._gpd.get_grasp_pose(grab_entity, arm) # - - - - - - - - - - - - - - - - - - - - - - - - - - - - goal_map = VectorStamped(0, 0, 0, frame_id=grab_entity.id) # In case grasp point determination didn't work if not grasp_framestamped: goal_bl = goal_map.projectToFrame(self.robot.robot_name + '/base_link', tf_listener=self.robot.tf_listener) if goal_bl is None: return 'failed' else: return 'failed' else: # We do have a grasp pose, given as a kdl frame in map try: self.robot.tf_listener.waitForTransform("/map", self.robot.robot_name + "/base_link", rospy.Time(0), rospy.Duration(10)) # Transform to base link frame goal_bl = grasp_framestamped.projectToFrame(self.robot.robot_name + "/base_link", tf_listener=self.robot.tf_listener) if goal_bl is None: return 'failed' except tf2_ros.TransformException as tfe: rospy.logerr('Transformation of goal to base failed: {0}'.format(tfe)) return 'failed' # Pre-grasp --> this is only necessary when using visual servoing # rospy.loginfo('Starting Pre-grasp') # if not arm.send_goal(goal_bl.x, goal_bl.y, goal_bl.z, 0, 0, 0, # frame_id='/'+self.robot.robot_name+'/base_link', # timeout=20, pre_grasp=True, first_joint_pos_only=True # ): # rospy.logerr('Pre-grasp failed:') # arm.reset() # arm.send_gripper_goal('close', timeout=None) # return 'failed' # Grasp rospy.loginfo('Start grasping') if not arm.send_goal(goal_bl, timeout=20, pre_grasp=True, allowed_touch_objects=[grab_entity.id]): self.robot.speech.speak('I am sorry but I cannot move my arm to the object position', block=False) rospy.logerr('Grasp failed') arm.reset() arm.send_gripper_goal('close', timeout=0.0) return 'failed' # Close gripper arm.send_gripper_goal('close') arm.occupied_by = grab_entity # Lift goal_bl = grasp_framestamped.projectToFrame(self.robot.robot_name + "/base_link", tf_listener=self.robot.tf_listener) rospy.loginfo('Start lifting') roll = 0.0 goal_bl.frame.p.z(goal_bl.frame.p.z() + 0.05) # Add 5 cm goal_bl.frame.M = kdl.Rotation.RPY(roll, 0, 0) # Update the roll rospy.loginfo("Start lift") if not arm.send_goal(goal_bl, timeout=20, allowed_touch_objects=[grab_entity.id]): rospy.logerr('Failed lift') # Retract goal_bl = grasp_framestamped.projectToFrame(self.robot.robot_name + '/base_link', tf_listener=self.robot.tf_listener) rospy.loginfo('Start retracting') roll = 0.0 goal_bl.frame.p.x(goal_bl.frame.p.x() - 0.1) # Retract 10 cm goal_bl.frame.p.z(goal_bl.frame.p.z() + 0.05) # Go 5 cm higher goal_bl.frame.M = kdl.Rotation.RPY(roll, 0.0, 0.0) # Update the roll rospy.loginfo("Start retract") if not arm.send_goal(goal_bl, timeout=0.0, allowed_touch_objects=[grab_entity.id]): rospy.logerr('Failed retract') arm.wait_for_motion_done() self.robot.base.force_drive(-0.125, 0, 0, 2.0) # Update Kinect once again to make sure the object disappears from ED segm_res = self.robot.ed.update_kinect("%s" % grab_entity.id) arm.wait_for_motion_done(cancel=True) # Carrying pose # rospy.loginfo('start moving to carrying pose') arm.send_joint_goal('carrying_pose', timeout=0.0) result = 'succeeded' if self._check_occupancy: # Check if the object is present in the gripper if arm.object_in_gripper_measurement.is_empty: # If state is empty, grasp has failed result = "failed" rospy.logerr("Gripper is not holding an object") self.robot.speech.speak("Whoops, something went terribly wrong") arm.occupied_by = None # Set the object the arm is holding to None else: # State is holding, grasp succeeded. # If unknown: sensor not there, assume gripper is holding and hope for the best result = "succeeded" if arm.object_in_gripper_measurement.is_unknown: rospy.logwarn("GripperMeasurement unknown") # Reset head self.robot.head.cancel_goal() return result