def get_detect_shelf_layers_path(self, shelf_system_id): """ :type shelf_system_id: str :rtype: FullBodyPath """ shelf_system_width = self.knowrob.get_shelf_layer_width( shelf_system_id) # shelf_system_height = self.knowrob.get_shelf_system_height(shelf_system_id) full_body_path = FullBodyPath() # via points for via_point in self.get_via_points(shelf_system_id): full_body_pose = FullBodyPosture() full_body_pose.base_pos = via_point full_body_pose.type = FullBodyPosture.BASE full_body_path.postures.append(full_body_pose) # calculate base pose base_pose = self.base_pose_in_front_of_shelf(shelf_system_id, shelf_system_width / 2, y=-0.85) base_pose = transform_pose( self.knowrob.get_perceived_frame_id(shelf_system_id), base_pose) if self.is_left(shelf_system_id): base_pose.pose.position.x += 0.5 else: base_pose.pose.position.x -= 0.5 base_pose = transform_pose('map', base_pose) full_body_pose = FullBodyPosture() full_body_pose.base_pos = base_pose full_body_pose.type = FullBodyPosture.BASE full_body_path.postures.append(full_body_pose) full_body_pose = FullBodyPosture() if self.is_left(shelf_system_id): full_body_pose.goal_joint_state = self.get_floor_detection_pose_left( ) else: full_body_pose.goal_joint_state = self.get_floor_detection_pose_right( ) full_body_pose.type = FullBodyPosture.JOINT full_body_path.postures.append(full_body_pose) full_body_pose = FullBodyPosture() full_body_pose.base_pos = self.cam_pose_in_front_of_shelf( shelf_system_id, x=shelf_system_width / 2, y=-0.55) full_body_pose.type = FullBodyPosture.CAM_FOOTPRINT full_body_path.postures.append(full_body_pose) full_body_pose = FullBodyPosture() full_body_pose.type = FullBodyPosture.CAMERA full_body_pose.camera_pos.header.frame_id = 'camera_link' full_body_pose.camera_pos.pose.position = Point(0, 1.364, 0) full_body_pose.camera_pos.pose.orientation = Quaternion(0, 0, 0, 1) full_body_path.postures.append(full_body_pose) return full_body_path
def stop_barcode_detection(self, frame_id): """ :type frame_id: str :return: dict mapping barcode to pose stamped :rtype: dict """ barcodes = {} width = self.knowrob.get_shelf_layer_width(self.current_shelf_layer_id) num_of_barcodes = max( 1, self.number_of_facings - int(np.random.rand() * 3)) for i in range(num_of_barcodes): barcode = PoseStamped() barcode.header.frame_id = self.knowrob.get_perceived_frame_id( self.current_shelf_layer_id) x = max( 0, min(width, ((i + .5) / (num_of_barcodes)) * width + np.random.normal(scale=.1 / num_of_barcodes))) barcode.pose.position = Point(x, 0, 0) barcode.pose.orientation.w = 1 barcode = transform_pose('map', barcode) try: if np.random.choice([True]): barcodes[str(self.barcodes.pop())] = barcode else: rnd_barcode = self.make_rnd_barcode() barcodes[rnd_barcode] = barcode except KeyError: self.get_all_barcodes() barcodes[self.barcodes.pop()] = barcode return barcodes
def cam_pose_in_front_of_shelf(self, shelf_system_id, frame_id=None, x=0., y=-0.55, x_limit=0.1, goal_angle=0.): """ computes a goal for base_link such that torso_Schwenker_Cams is at x/y in front of shelf_system_id :type shelf_system_id: str :type x: float :type y: float :param x_limit: safety buffer range along x axis of shelf system :type x_limit: float :rtype: PoseStamped """ width = self.knowrob.get_shelf_system_width(shelf_system_id) cam_pose = PoseStamped() if frame_id is None: cam_pose.header.frame_id = self.knowrob.get_perceived_frame_id( shelf_system_id) else: cam_pose.header.frame_id = frame_id cam_pose.pose.position.x = max(0 + x_limit, min(width - x_limit, x)) cam_pose.pose.position.y = np.cos(goal_angle) * y cam_pose.pose.orientation = self.get_goal_base_rotation( shelf_system_id) cam_pose = transform_pose('map', cam_pose) cam_pose.pose.position.z = 0 return cam_pose
def see(self, depth, width, height): q = {'detect': {'pose': ''}} self.print_with_prefix('sending: {}'.format(q)) req = RSQueryServiceRequest() req.query = json.dumps(q) rospy.sleep(0.4) objects = [] expected_volume = self.volume(depth, width, height) while len(objects) < 5: result = self.robosherlock_service.call(req) answers = [json.loads(x) for x in result.answer] if not answers: continue real_object = min( answers, key=lambda x: abs(self.answer_volume(x) - expected_volume)) if abs( 1 - self.answer_front_area(real_object) / (width * height)) > 0.3: #175: # reject if more than x% diff continue try: pose = message_converter.convert_dictionary_to_ros_message( 'geometry_msgs/PoseStamped', real_object['poses'][0]['pose_stamped']) except: continue pose.pose.position.z += real_object['boundingbox'][ 'dimensions-3D']['height'] / 2. objects.append(pose) self.print_with_prefix('found pose number {}'.format(len(objects))) objects = self.filter_outlier(objects) p = self.avg_pose(objects) # p.pose.position.z += depth/2. return transform_pose('map', p)
def move_other_frame(self, target_pose, frame='camera_link', retry=True): if self.enabled: target_pose = self.cam_pose_to_base_pose(target_pose, frame) target_pose = transform_pose('map', target_pose) target_pose.pose.position.z = 0 self.goal_pub.publish(target_pose) goal = MoveBaseGoal() goal.target_pose = target_pose if self.knowrob is not None: self.knowrob.start_base_movement(self.knowrob.pose_to_prolog(target_pose)) while True: self.client.send_goal(goal) wait_result = self.client.wait_for_result(rospy.Duration(self.timeout)) result = self.client.get_result() state = self.client.get_state() if wait_result and state == GoalStatus.SUCCEEDED: break if retry: cmd = raw_input('base movement did not finish in time, retry? [y/n]') retry = cmd == 'y' if not retry: print('movement did not finish in time') if self.knowrob is not None: self.knowrob.finish_action() raise TimeoutError() if self.knowrob is not None: self.knowrob.finish_action() return result
def update_shelf_system_pose(knowrob, top_layer_id, separators): """ :type knowrob: refills_perception_interface.knowrob_wrapper.KnowRob :type shelf_system_id: str :type separators: list :return: """ if not knowrob.is_bottom_layer(top_layer_id): return if len(separators) == 0: return shelf_system_id = knowrob.get_shelf_system_from_layer(top_layer_id) shelf_system_frame_id = knowrob.get_object_frame_id(shelf_system_id) separators_in_system = [ transform_pose(shelf_system_frame_id, p) for p in separators ] separators_xy = np.array([[p.pose.position.x, p.pose.position.y] for p in separators_in_system]) separators_y = np.array(separators_xy)[:, 1].mean() T_system___layer = lookup_pose( shelf_system_frame_id, knowrob.get_perceived_frame_id(top_layer_id)) y_offset = separators_y - T_system___layer.pose.position.y A = np.vstack( [np.array(separators_xy)[:, 0], np.ones(separators_xy.shape[0])]).T y = separators_xy[:, 1] m, _ = np.linalg.lstsq(A, y, rcond=-1)[0] x = np.array([1, m, 0]) x = x / np.linalg.norm(x) z = np.array([0, 0, 1]) y = np.cross(z, x) q = PyKDL.Rotation(x[0], y[0], z[0], x[1], y[1], z[1], y[2], y[2], z[2]).GetQuaternion() offset = PoseStamped() offset.header.frame_id = shelf_system_frame_id offset.pose.position.y = y_offset offset.pose.orientation = Quaternion(*q) offset = transform_pose('map', offset) knowrob.belief_at_update(shelf_system_id, offset) rospy.sleep(0.5)
def cb(self, data): """ fills a dict that maps barcode to list of PoseStamped where it was seen. :type data: Barcode """ if self.listen: p = transform_pose(MAP, data.barcode_pose) if p is not None: p.header.stamp = rospy.Time() if p is not None and self.barcode_on_shelf_layer(p): if data.barcode[0] == '2': self.barcodes[data.barcode[1:-1]].append(p)
def get_count_product_posture(self, facing_id): """ :type facing_id: str :rtype: FullBodyPosture """ shelf_layer_id = self.knowrob.get_shelf_layer_from_facing(facing_id) shelf_system_id = self.knowrob.get_shelf_system_from_layer( shelf_layer_id) # shelf_system_width = self.knowrob.get_shelf_system_width(shelf_system_id) # shelf_system_frame_id = self.knowrob.get_perceived_frame_id(shelf_system_id) shelf_layer_frame_id = self.knowrob.get_perceived_frame_id( shelf_layer_id) facing_frame_id = self.knowrob.get_object_frame_id(facing_id) # get height of next layer # shelf_layer_above_id = self.knowrob.get_shelf_layer_above(shelf_layer_id) # if shelf_layer_above_id is not None: # shelf_layer_above_frame_id = self.knowrob.get_perceived_frame_id(shelf_layer_above_id) # height_of_next_layer = lookup_transform('map', shelf_layer_above_frame_id).pose.position.z # else: # height_of_next_layer = self.knowrob.get_shelf_system_height(shelf_system_id) # joints shelf_layer_height = lookup_pose('map', shelf_layer_frame_id).pose.position.z counting_offset = 0.3 # TODO tune this number torso_rot_1_height = shelf_layer_height + counting_offset torso_rot_1_height = max(MIN_CAM_HEIGHT, torso_rot_1_height) goal_angle = radians(-20) full_body_pose = self.get_cam_pose(torso_rot_1_height, goal_angle, self.is_left(shelf_system_id)) facing_pose_on_layer = lookup_pose(shelf_layer_frame_id, facing_frame_id) # base_pose = self.cam_pose_in_front_of_facing(facing_id, x=0, x_limit=0, goal_angle=goal_angle) base_pose = self.cam_pose_in_front_of_layer( shelf_layer_id, x=facing_pose_on_layer.pose.position.x, y=-.55, goal_angle=goal_angle) base_pose = transform_pose('map', base_pose) full_body_pose.type = full_body_pose.BOTH full_body_pose.base_pos = base_pose return full_body_pose
def separator_cb(self, separator_array): """ adds detected separators to self.detections :type separator_array: SeparatorArray """ if self.listen: # print('registered separator') for separator in separator_array.separators: p = transform_pose('map', separator.separator_pose) p.header.stamp = rospy.Time() # p = transform_pose(self.current_frame_id, p) # rospy.logwarn('cant transform separator') if p is not None and self.separator_on_shelf_layer(p): self.detections.append([ p.pose.position.x, p.pose.position.y, p.pose.position.z ])
def stop_separator_detection(self, frame_id): """ :type frame_id: str :return: list of pose stamps :rtype: list """ separators = [] width = self.knowrob.get_shelf_layer_width(self.current_shelf_layer_id) for i in range(self.number_of_facings + 1): separator = PoseStamped() separator.header.frame_id = self.knowrob.get_perceived_frame_id( self.current_shelf_layer_id) x = (i / (self.number_of_facings)) * width if x > 0.01 and x < 0.99: x = max(0, min(width, x + np.random.normal(scale=0.02))) separator.pose.position = Point(x, 0, 0) separator.pose.orientation.w = 1 separator = transform_pose('map', separator) separators.append(separator) return separators
def get_facing_ids_from_layer(self, shelf_layer_id): """ :type shelf_layer_id: str :return: :rtype: OrderedDict """ shelf_system_id = self.get_shelf_system_from_layer(shelf_layer_id) q = 'findall([F, P], (shelf_facing(\'{}\', F),is_at(F, P)), Fs).'.format( shelf_layer_id) solutions = self.all_solutions(q)[0] facings = [] for facing_id, pose in solutions['Fs']: facing_pose = self.prolog_to_pose_msg(pose) facing_pose = transform_pose( self.get_perceived_frame_id(shelf_layer_id), facing_pose) facings.append((facing_id, facing_pose)) is_left = 1 if self.is_left(shelf_system_id) else -1 facings = list( sorted(facings, key=lambda x: x[1].pose.position.x * is_left)) return OrderedDict(facings)
def update_shelf_layer_position(self, shelf_layer_id, separators): """ :type shelf_layer_id: str :type separators: list of PoseStamped, positions of separators """ if len(separators) > 0: old_p = lookup_pose('map', self.get_perceived_frame_id(shelf_layer_id)) separator_zs = [p.pose.position.z for p in separators] new_floor_height = np.mean(separator_zs) current_floor_pose = lookup_pose( MAP, self.get_object_frame_id(shelf_layer_id)) current_floor_pose.pose.position.z += new_floor_height - old_p.pose.position.z shelf_system = self.get_shelf_system_from_layer(shelf_layer_id) frame_id = self.get_object_frame_id(shelf_system) current_floor_pose.header.stamp = rospy.Time() current_floor_pose = transform_pose(frame_id, current_floor_pose) q = 'tell(is_at(\'{}\', {}))'.format( shelf_layer_id, self.pose_to_prolog(current_floor_pose)) self.once(q)
def stop_detect_shelf_layers(self, shelf_system_id): """ :type shelf_system_id: str :return: list of shelf layer heights :rtype: list """ shelf_frame = self.knowrob.get_perceived_frame_id(shelf_system_id) req = RSQueryServiceRequest() q = { 'scan': { 'type': 'shelf', 'location': shelf_frame, 'command': 'stop' } } req.query = json.dumps(q) self.print_with_prefix('sending: {}'.format(q)) result = self.robosherlock_service.call(req) self.print_with_prefix('received: {}'.format(result)) floors = [] for floor in result.answer: pose = json.loads(floor)['rs.annotation.PoseAnnotation'][0][ 'camera']['rs.tf.StampedPose'] p = self.rs_pose_to_geom_msgs_pose(pose) # p = message_converter.convert_dictionary_to_ros_message('geometry_msgs/PoseStamped', # json.loads(floor)['poses'][0]['pose_stamped']) # TODO potential speedup, safe and reuse transform p = transform_pose(shelf_frame, p) floors.append(p.pose.position.z) floors = list(sorted(floors)) # floors = [x[-1] for x in floors] floors = add_bottom_layer_if_not_present(floors, shelf_system_id, self.knowrob) return floors
def get_detect_facings_path(self, shelf_layer_id): """ :type shelf_layer_id: str :rtype: FullBodyPath """ shelf_system_id = self.knowrob.get_shelf_system_from_layer( shelf_layer_id) # shelf_system_frame_id = self.knowrob.get_perceived_frame_id(shelf_system_id) shelf_system_width = self.knowrob.get_shelf_system_width( shelf_system_id) shelf_layer_frame_id = self.knowrob.get_perceived_frame_id( shelf_layer_id) full_body_path = FullBodyPath() # TODO consider left right cases # joints # TODO layer above shelf_layer_height = lookup_pose('map', shelf_layer_frame_id).pose.position.z # TODO tune this number to_low_offset = 0.05 other_offset = 0.05 full_body_pose = FullBodyPosture() full_body_pose.type = FullBodyPosture.CAMERA if self.layer_too_low(shelf_layer_height): goal_angle = radians(-20) full_body_pose = self.get_cam_pose( shelf_layer_height + to_low_offset, goal_angle, self.is_left(shelf_system_id)) else: goal_angle = radians(0) full_body_pose = self.get_cam_pose( shelf_layer_height + other_offset, goal_angle, self.is_left(shelf_system_id)) full_body_path.postures.append(full_body_pose) # base poses # start base poses start_base_pose = self.cam_pose_in_front_of_layer( shelf_layer_id, goal_angle=goal_angle) start_base_pose = transform_pose('map', start_base_pose) start_full_body_pose = FullBodyPosture() start_full_body_pose.type = FullBodyPosture.CAM_FOOTPRINT start_full_body_pose.base_pos = start_base_pose end_base_pose = self.cam_pose_in_front_of_layer(shelf_layer_id, x=shelf_system_width, goal_angle=goal_angle) end_base_pose = transform_pose('map', end_base_pose) end_full_body_pose = FullBodyPosture() end_full_body_pose.type = FullBodyPosture.CAM_FOOTPRINT end_full_body_pose.base_pos = end_base_pose if self.is_left(shelf_system_id): full_body_path.postures.append(end_full_body_pose) full_body_path.postures.append(start_full_body_pose) else: full_body_path.postures.append(start_full_body_pose) full_body_path.postures.append(end_full_body_pose) return full_body_path