manager.py 文件源码

def _pick(self):
        # State not modified until picking succeeds
        try:
            obj_to_get = int(self.character)
        except ValueError:
            rospy.logerr("Please provide a number in picking mode")
            return

        frame_name = "object_pose_{}".format(obj_to_get)

        rospy.loginfo("Picking object {}...".format(obj_to_get))
        if self.tl.frameExists(self.robot.base) and self.tl.frameExists(frame_name):
            t = self.tl.getLatestCommonTime(self.robot.base, frame_name)
            position, quaternion = self.tl.lookupTransform(self.robot.base,
                                                           frame_name,
                                                           t)
            print("position", position)
            print("quaternion", quaternion)

            position = list(position)
            # Vertical orientation
            self.br.sendTransform(position,
                                  [1, 0, 0, 0],
                                  rospy.Time.now(),
                                  "pick_pose",
                                  self.robot.base)
            # Ignore orientation from perception
            pose = Pose(Point(*position),
                        Quaternion(1, 0, 0, 0))
            h = Header()
            h.stamp = t
            h.frame_id = self.robot.base
            stamped_pose = PoseStamped(h, pose)
            self.robot.pick(stamped_pose)
            self.state = 'postpick'