python类Quaternion()的实例源码

def quat_to_angle(quat):
    rot = PyKDL.Rotation.Quaternion(quat.x, quat.y, quat.z, quat.w)
    return rot.GetRPY()[2]

def quat_to_angle(quat):
    rot = PyKDL.Rotation.Quaternion(quat.x, quat.y, quat.z, quat.w)
    return rot.GetRPY()[2]

def get_odom(self):
        # Get the current transform between the odom and base frames
        try:
            (trans, rot)  = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0))
        except (tf.Exception, tf.ConnectivityException, tf.LookupException):
            rospy.loginfo("TF Exception")
            return

        return (Point(*trans), quat_to_angle(Quaternion(*rot)))

def get_ros_quaternion(self, almath_quaternion):
        output = Quaternion()
        output.w = almath_quaternion.w
        output.x = almath_quaternion.x
        output.y = almath_quaternion.y
        output.z = almath_quaternion.z
        return output

def q_to_msg_q(v):
    return Quaternion(v[0], v[1], v[2], v[3])

def get_lean_points(self, angle):
        """ Return a set of trajectory points to accomplish a lean """
        point = SO3TrajectoryPointRosMessage()

        point.time = 1.0  # Just give it a second to get there
        rotate = quaternion_about_axis(radians(angle), [0, 0, -1])
        point.orientation = Quaternion(rotate[1], rotate[2], rotate[3], rotate[0])
        point.angular_velocity = Vector3(0, 0, 0)
        log('Lean to: {}'.format(point))

        points = []
        points.append(point)
        return points

def lean_body_to(self, angle, wait=True):
        """ Lean forward or back to a given angle """
        chest_position = self.zarj.transform.lookup_transform(
            'world', 'torso', rospy.Time()).transform

        log("Lean body to {} degrees".format(angle))
        msg = ChestTrajectoryRosMessage()
        msg.unique_id = self.zarj.uid

        msg.execution_mode = msg.OVERRIDE

        euler = euler_from_quaternion((chest_position.rotation.w,
                                       chest_position.rotation.x,
                                       chest_position.rotation.y,
                                       chest_position.rotation.z))

        point = SO3TrajectoryPointRosMessage()
        point.time = 1.0
        point.orientation = chest_position.rotation

        qua = quaternion_from_euler(euler[0], radians(angle), euler[2])
        point.orientation = Quaternion(qua[1], qua[2], qua[3], qua[0])
        point.angular_velocity = Vector3(0.0, 0.0, 0.0)

        msg.taskspace_trajectory_points = [point]

        self.chest_publisher.publish(msg)
        if wait:
            rospy.sleep(point.time + 0.2)

def turn_body(self, angle):
        """
            Turn the torso by a given angle

            Angles smaller than 110 or larger than 250 are unstable
        """
        log("Turn body {} degrees".format(angle))
        chest_position = self.zarj.transform.lookup_transform(
            'world', 'torso', rospy.Time()).transform

        msg = ChestTrajectoryRosMessage()
        msg.unique_id = self.zarj.uid

        msg.execution_mode = msg.OVERRIDE

        euler = euler_from_quaternion((chest_position.rotation.w,
                                       chest_position.rotation.x,
                                       chest_position.rotation.y,
                                       chest_position.rotation.z))

        point = SO3TrajectoryPointRosMessage()
        point.time = 1.0

        qua = quaternion_from_euler(euler[0] + radians(angle), euler[1],
                                    euler[2])
        point.orientation = Quaternion(qua[1], qua[2], qua[3], qua[0])
        point.angular_velocity = Vector3(0.0, 0.0, 0.0)

        msg.taskspace_trajectory_points = [point]

        self.chest_publisher.publish(msg)

def turn_body_to(self, angle, wait=True):
        """
            Turn the torso to a given angle as related to the pelvis

            Angles smaller than 110 or larger than 250 are unstable
        """
        log("Turn body to {} degrees".format(angle))
        chest_position = self.zarj.transform.lookup_transform(
            'pelvis', 'torso', rospy.Time()).transform

        msg = ChestTrajectoryRosMessage()
        msg.unique_id = self.zarj.uid

        msg.execution_mode = msg.OVERRIDE

        euler = euler_from_quaternion((chest_position.rotation.w,
                                       chest_position.rotation.x,
                                       chest_position.rotation.y,
                                       chest_position.rotation.z))

        qua = quaternion_from_euler(radians(-angle-180), euler[1], euler[2])

        pose = PoseStamped()
        pose.pose.orientation = Quaternion(qua[1], qua[2], qua[3], qua[0])
        pose.header.frame_id = 'pelvis'
        pose.header.stamp = rospy.Time()
        result = self.zarj.transform.tf_buffer.transform(pose, 'world')


        point = SO3TrajectoryPointRosMessage()
        point.time = 1.0
        point.orientation = result.pose.orientation
        point.angular_velocity = Vector3(0.0, 0.0, 0.0)

        msg.taskspace_trajectory_points = [point]

        self.chest_publisher.publish(msg)

        if wait:
            rospy.sleep(point.time + 0.1)

def update_camera_transform(self):
        t = self.tl.getLatestCommonTime("/root", "/camera_link")
        position, quaternion = self.tl.lookupTransform("/root",
                                                       "/camera_link",
                                                       t)
        position, quaternion = self.update_transformation(list(position), list(quaternion))

        #get ICP to find rotation and translation
        #multiply old position and orientation by rotation and translation
        #publish new pose to /update_camera_alignment
        self.update_transform.publish( Pose(Point(*position), Quaternion(*quaternion)))

def move_calib_position(self):
        # move arm to the calibration position
        self.calib_pose = PoseStamped(
            Header(0, rospy.Time(0), self.robot.base),
            Pose(Point(0.338520675898,-0.175860479474,0.0356775075197),
                 Quaternion(-0.0183493755758,0.708424150944, 0.704712092876, 0.0343413949013)))
        self.robot.move_ik(self.calib_pose)

def orientation_from_quaternion(q):
    return Quaternion(*q)

def make_empty_pose():
    return Pose(
        Point(0, 0, 0),
        Quaternion(0, 0, 0, 1)
    )

def angle_to_quaternion(angle):
    """Convert an angle in radians into a quaternion _message_."""
    return Quaternion(*tf.transformations.quaternion_from_euler(0, 0, angle))

def numpy_to_odom(arr, frame_id=None):
    """
    Takes in a numpy array [x,y,theta] and a frame ID and converts it to an
    Odometry message. 

    WARN: May require a timestamp.
    """
    odom = Odometry()
    if frame_id is not None: odom.header.frame_id = frame_id
    odom.pose.pose.position.x = arr[0]
    odom.pose.pose.position.y = arr[1]
    odom.pose.pose.orientation = Quaternion(*tr.quaternion_from_euler(arr[2], 0, 0, 'szyx'))
    return odom

def get_odom(self):
        # Get the current transform between the odom and base frames
        try:
            (trans, rot)  = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0))
        except (tf.Exception, tf.ConnectivityException, tf.LookupException):
            rospy.loginfo("TF Exception")
            return

        return (Point(*trans), quat_to_angle(Quaternion(*rot)))

def get_odom(self):
        # Get the current transform between the odom and base frames
        try:
            (trans, rot)  = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0))
        except (tf.Exception, tf.ConnectivityException, tf.LookupException):
            rospy.loginfo("TF Exception")
            return

        return (Point(*trans), quat_to_angle(Quaternion(*rot)))

def get_odom_angle(self):
        # Get the current transform between the odom and base frames
        try:
            (trans, rot)  = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0))
        except (tf.Exception, tf.ConnectivityException, tf.LookupException):
            rospy.loginfo("TF Exception")
            return

        # Convert the rotation from a quaternion to an Euler angle
        return quat_to_angle(Quaternion(*rot))

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()