python类Pose()的实例源码

def close_gripper(self):
        """Send the instruction to the robot to close the gripper."""
        goal = Pose()
        goal_final = baxterGoal(id=3, pose=goal)
        status = self.left_client.send_goal_and_wait(goal_final)
        result = self.left_client.wait_for_result()
        return result

def open_gripper(self):
        """Send the instruction to the robot to open the gripper."""
        goal = Pose()
        goal_final = baxterGoal(id=2, pose=goal)
        self.left_client.send_goal_and_wait(goal_final)
        result = self.left_client.wait_for_result()
        return result

def test_relative(self):
        """Test robot's ability to position its gripper relative to a given marker."""
        goal = Pose()
        roll = -math.pi / 2
        pitch = 0
        yaw = -math.pi / 2
        quaternion = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
        goal.orientation.x = quaternion[0]
        goal.orientation.y = quaternion[1]
        goal.orientation.z = quaternion[2]
        goal.orientation.w = quaternion[3]
        while True:
            end = user_input("Zelite li nastaviti? d/n")
            if end != 'd':
                break
            trans = self.transformations()
            goal.position.x = trans[0]
            goal.position.y = trans[1]
            goal.position.z = trans[2]
            offset_x = float(user_input("X?"))
            offset_y = float(user_input("Y?"))
            offset_z = float(user_input("Z?"))
            # Uncomment for testing movement speed as well
            # brzina = float(user_input("Brzina?"))
            # self.left_arm.set_joint_position_speed(brzina)
            goal.position.x += offset_x
            goal.position.y += offset_y
            goal.position.z += offset_z
            goal_final = baxterGoal(id=1, pose=goal)
            self.left_client.send_goal_and_wait(goal_final)
            result = self.left_client.get_result()

def __init__(self):
        rospy.init_node("speech")
        rospy.on_shutdown(self.shutdown)

        self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)

        rospy.loginfo("Waiting for move_base action server...")

        # Wait up to 60 seconds for the action server to become available
        self.move_base.wait_for_server(rospy.Duration(60))    

        self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5)
        nav_goal = MoveBaseGoal()
        nav_goal.target_pose.header.frame_id = 'base_link'
        nav_goal.target_pose.header.stamp = rospy.Time.now()
        q_angle = quaternion_from_euler(0, 0,0, axes='sxyz')
        q = Quaternion(*q_angle)
        nav_goal.target_pose.pose =Pose( Point(0.3,0,0),q)
        move_base_state = SimpleActionState('move_base', MoveBaseAction, goal=nav_goal, result_cb=self.move_base_result_cb, 
                                                exec_timeout=rospy.Duration(60.0),
                                                server_wait_timeout=rospy.Duration(60.0))


        smach_top=StateMachine(outcomes=['succeeded','aborted','preempted',"valid","invalid"])
        with smach_top:
            StateMachine.add("Wait_4_Statr", MonitorState("task_comming", xm_Task, self.start_cb),
                                                  transitions={'invalid':'NAV',
                                                               'valid':'Wait_4_Statr',
                                                               'preempted':'NAV'})

            StateMachine.add("NAV", move_base_state, transitions={"succeeded":"Wait_4_Stop","aborted":"NAV","preempted":"Wait_4_Stop"})
            StateMachine.add("Wait_4_Stop",MonitorState("task_comming", xm_Task, self.stop_cb),
                                                  transitions={'invalid':'',
                                                               'valid':'Wait_4_Stop',
                                                               'preempted':''})

            smach_top.execute()

def people_msg_cb(self,msg):
        self.people_msg=msg
        if self.people_msg.pos is not None:
            goal = MoveBaseGoal()
            goal.target_pose.header.frame_id = 'base_link'
            q_angle = quaternion_from_euler(0, 0,0)
            self.q=Quaternion(*q_angle)
            goal.target_pose.pose=(Pose(Point(self.people_msg.pos.x-0.5,self.people_msg.pos.y,self.people_msg.pos.z),self.q))
            self.move_base.send_goal(goal)
            rospy.spin()

def run(self):
        global FLAG,PEOPLE_POSITION
        lock = threading.Lock()
#         while True:
        if FLAG!=0:
                with lock:
                    rospy.loginfo(PEOPLE_POSITION)
                    x=PEOPLE_POSITION.x-0.7
                    y=PEOPLE_POSITION.y
                self.goal.target_pose.pose=(Pose(Point(x,y,0),self.q))
                subprocess.call(["rosservice","call","/move_base/clear_costmaps"])
                self.move_base.send_goal(self.goal)
                FLAG=0
                rospy.sleep(0.1)
        return TaskStatus.RUNNING

def __init__(self):
        rospy.init_node('openhmd_ros')
        self.pub = rospy.Publisher('/openhmd/pose', Pose)
        self.hmd = None

def run(self):
        r = rospy.Rate(10)
        while not rospy.is_shutdown():
            d = self.hmd.poll()
            if len(d) != 4:
                continue

            pose = Pose()
            pose.orientation = Quaternion(d[0], d[1], d[2], d[3])

            euler = euler_from_quaternion(d)
            pose.position = Point(euler[0], euler[1], euler[2])

            self.pub.publish(pose)
            r.sleep()

def getPose(self):
        p=self.endpoint_pose()
        if len(p.keys())==0:
            rospy.logerr("ERROR: Pose is empty, you may want to wait a bit before calling getPose to populate data")
            return None
        pose=Pose()        
        pose.position=Point(*p["position"])
        pose.orientation=Quaternion(*p["orientation"])
        return pose

def getAnglesFromPose(self,pose):
        if type(pose)==Pose:
            goal=PoseStamped()
            goal.header.frame_id="/base"
            goal.pose=pose
        else:
            goal=pose


        if not self.ik:
            rospy.logerror("ERROR: Inverse Kinematics service was not loaded")
            return None
        goalstr="%f,%f,%f"%(goal.pose.position.x,goal.pose.position.y,goal.pose.position.z)
        ikreq = SolvePositionIKRequest()

        ikreq.pose_stamp.append(goal)
        try:
            resp = self.iksvc(ikreq)
            if (resp.isValid[0]):
                return resp.joints[0]
            else:
                rospy.logerr("FAILURE - No Valid Joint Solution Found for %s"%goalstr)
                return None
        except rospy.ServiceException,e :
            rospy.loginfo("Service call failed: %s" % (e,))
            return None

def particle_to_pose(particle):
    pose = Pose()
    pose.position.x = particle[0]
    pose.position.y = particle[1]
    pose.orientation = angle_to_quaternion(particle[2])
    return pose

def particle_to_pose(particle):
    pose = Pose()
    pose.position.x = particle[0]
    pose.position.y = particle[1]
    pose.orientation = angle_to_quaternion(particle[2])
    return pose

def __init__(self, id, position, quaternion):
        self.id = id
        self.pose = Pose(position, quaternion);

def create_pose(location, rotation):
    if len(location) >= 3:
        x, y, z = location[0:3]
    else:
        x = y = z = 0
    if len(rotation) >= 4:
        qw, qx, qy, qz = rotation[0:4]
    else:
        qw = 1
        qx = qy = qz = 0

    return geom_msgs.Pose(
        position=geom_msgs.Point(x=x, y=y, z=z),
        orientation=geom_msgs.Quaternion(w=qw, x=qx, y=qy, z=qz)
    )

def set_pose_targets(self, poses, end_effector_link = ""):
        """ Set the pose of the end-effector, if one is available. The expected input is a list of poses. Each pose can be a Pose message, a list of 6 floats: [x, y, z, rot_x, rot_y, rot_z] or a list of 7 floats [x, y, z, qx, qy, qz, qw] """
        if len(end_effector_link) > 0 or self.has_end_effector_link():
            if not self._g.set_pose_targets([conversions.pose_to_list(p) if type(p) is Pose else p for p in poses], end_effector_link):
                raise MoveItCommanderException("Unable to set target poses")
        else:
            raise MoveItCommanderException("There is no end effector to set poses for")

def get_object_poses(self, object_ids):
        """
        Get the poses from the objects identified by the given object ids list.
        """
        ser_ops = self._psi.get_object_poses(object_ids)
        ops = dict()
        for key in ser_ops:
            msg = Pose()
            conversions.msg_from_string(msg, ser_ops[key])
            ops[key] = msg
        return ops

def to_message(self):
        """ Return a nav_msgs/OccupancyGrid representation of this map. """

        grid_msg = OccupancyGrid()

        # Set up the header.
        grid_msg.header.stamp = rospy.Time.now()
        grid_msg.header.frame_id = "map"

        # .info is a nav_msgs/MapMetaData message.
        grid_msg.info.resolution = self.resolution
        grid_msg.info.width = self.width
        grid_msg.info.height = self.height

        # Rotated maps are not supported... quaternion represents no
        # rotation.
        grid_msg.info.origin = Pose(Point(self.origin_x, self.origin_y, 0.),
                               Quaternion(0., 0., 0., 1.))

        # Flatten the numpy array into a list of integers from 0-100.
        # This assumes that the grid entries are probalities in the
        # range 0-1. This code will need to be modified if the grid
        # entries are given a different interpretation (like
        # log-odds).
        flat_grid = self.grid.reshape((self.grid.size,)) * 100.
        grid_msg.data = list(np.round(flat_grid, decimals = 3))
        return grid_msg

def dict_to_msg(pose_dict):
    pose_msg = Pose()
    pose_msg.position = pose_dict['position']
    pose_msg.orientation = pose_dict['orientation']
    return pose_msg

def dragEnterEvent(self, event):
        if event.mimeData().hasText():
            topic_name = str(event.mimeData().text())
            if len(topic_name) == 0:
                qWarning('PoseViewWidget.dragEnterEvent(): event.mimeData() text is empty')
                return
        else:
            if not hasattr(event.source(), 'selectedItems') or len(event.source().selectedItems()) == 0:
                qWarning('PoseViewWidget.dragEnterEvent(): event.source() has no attribute selectedItems or length of selectedItems is 0')
                return
            item = event.source().selectedItems()[0]
            topic_name = item.data(0, Qt.UserRole)

            if topic_name is None:
                qWarning('PoseViewWidget.dragEnterEvent(): selectedItem has no UserRole data with a topic name')
                return

        # check for valid topic
        msg_class, self._topic_name, _ = get_topic_class(topic_name)
        if msg_class is None:
            qWarning('PoseViewWidget.dragEnterEvent(): No message class was found for topic "%s".' % topic_name)
            return

        # check for valid message class
        quaternion_slot_path, point_slot_path = self._get_slot_paths(msg_class)

        if quaternion_slot_path is None and point_slot_path is None:
            qWarning('PoseViewWidget.dragEnterEvent(): No Pose, Quaternion or Point data was found outside of arrays in "%s" on topic "%s".'
                     % (msg_class._type, topic_name))
            return

        event.acceptProposedAction()

def _get_slot_paths(msg_class):
        # find first Pose in msg_class
        pose_slot_paths = find_slots_by_type_bfs(msg_class, Pose)
        for path in pose_slot_paths:
            # make sure the path does not contain an array, because we don't want to deal with empty arrays...
            if '[' not in path:
                path = PoseViewWidget._make_path_list_from_path_string(pose_slot_paths[0])
                return path + ['orientation'], path + ['position']

        # if no Pose is found, find first Quaternion and Point
        quaternion_slot_paths = find_slots_by_type_bfs(msg_class, Quaternion)
        for path in quaternion_slot_paths:
            if '[' not in path:
                quaternion_slot_path = PoseViewWidget._make_path_list_from_path_string(path)
                break
        else:
            quaternion_slot_path = None

        point_slot_paths = find_slots_by_type_bfs(msg_class, Point)
        for path in point_slot_paths:
            if '[' not in path:
                point_slot_path = PoseViewWidget._make_path_list_from_path_string(path)
                break
        else:
            point_slot_path = None

        return quaternion_slot_path, point_slot_path