python类Quaternion()的实例源码

def GetWayPoints(self,Data):
        filename = Data.data
        #clear all existing marks in rviz
        for marker in self.makerArray.markers:
            marker.action = Marker.DELETE
        self.makerArray_pub.publish(self.makerArray)

        # clear former lists
        self.waypoint_list.clear()
        self.marker_list[:] = []
        self.makerArray.markers[:] = []

        f = open(filename,'r')
        for line in f.readlines():
            j = line.split(",")
            current_wp_name     = str(j[0])        
            current_point       = Point(float(j[1]),float(j[2]),float(j[3]))
            current_quaternion  = Quaternion(float(j[4]),float(j[5]),float(j[6]),float(j[7]))

            self.waypoint_list[current_wp_name] = Pose(current_point,current_quaternion)

        f.close()

        self.create_markers()
        self.makerArray_pub.publish(self.makerArray)

def odom_add_offset(odom, offset):
    """
    Takes in two odometry messages and returns a new odometry message that has
    the two added together.

    WARN: Both messages should be in the same frame!
    """
    new_odom = copy.deepcopy(odom)
    new_odom.pose.pose.position.x += offset.pose.pose.position.x
    new_odom.pose.pose.position.y += offset.pose.pose.position.y
    quat = new_odom.pose.pose.orientation
    quat_offset = offset.pose.pose.orientation
    quat_arr = np.array([quat.x, quat.y, quat.z, quat.w])
    offset_arr = np.array([quat_offset.x, quat_offset.y, quat_offset.z, quat_offset.w])
    theta = tr.euler_from_quaternion(quat_arr, 'sxyz')[2]
    theta_offset = tr.euler_from_quaternion(offset_arr, 'sxyz')[2]
    new_odom.pose.pose.orientation = Quaternion(*tr.quaternion_from_euler(theta+theta_offset, 0, 0, 'szyx'))
    return new_odom

def fmt(obj, nest_level=0):
    """ Format any common object """
    if nest_level > 10:
        return ""
    if isinstance(obj, float):
        return "{0:.3f}".format(obj)
    if isinstance(obj, list):
        return "(" + ",".join(map(lambda x: fmt(x, nest_level + 1), obj)) + ")"
    if isinstance(obj, (numpy.ndarray, numpy.generic)):
        return fmt(obj.tolist(), nest_level + 1)
    if isinstance(obj, dict):
        pairs = map(lambda x, y: "(" + fmt(x) + "," + fmt(y, nest_level + 1) + ")", obj.items())
        return fmt(pairs)
    if isinstance(obj, Vector3):
        return "({},{},{})".format(fmt(obj.x), fmt(obj.y), fmt(obj.z))
    if isinstance(obj, Quaternion):
        return "({},{},{},{})".format(fmt(obj.x), fmt(obj.y), fmt(obj.z), fmt(obj.w))
    # print " obj " + str(obj) + " is of type " + str(type(obj))
    return str(obj)

def determine_hand_move_time(self, sidename, position, orientation, cur_transform):
        move_time = self.HAND_TRAJECTORY_TIME
        desired_q = msg_q_to_q(orientation)
        cur_q = msg_q_to_q(cur_transform.rotation)
        rotation_amount = quaternion_multiply(desired_q, conjugate_q(cur_q))
        rot_w = rotation_amount[3]
        if rot_w < 0: rot_w = -rot_w
        add_time = 0
        if rot_w < 0.85:
            add_time = self.HAND_ROTATION_TIME
        # if rot_w < 0.92
        #    add_time = self.HAND_ROTATION_TIME
        # if rot_w < 0.75:
        #    add_time += self.HAND_ROTATION_TIME
        if add_time > 0:
            rospy.loginfo('Doing significant {} hand rotation, '
                          'adding rotation wait of {}.'.format(sidename, add_time))
            move_time += add_time
        return move_time

    # Moves the hand center to an absolute world position (Vector3) and orientation (Quaternion).

def update_translation(self):
        t = self.tl.getLatestCommonTime("/root", "/camera_link")
        position, quaternion = self.tl.lookupTransform("/root",
                                                       "/camera_link",
                                                       t)
        print("Cam Pos:")
        print(position)

        translation = self.touch_centroids[0] - self.camera_centroids[0]
        print("Translation: ")
        print(translation)
        position = position + translation
        print("New Cam Pos:")
        print(position)
        #print(self.touch_centroids)
        #print(self.camera_centroids)

        self.update_transform.publish( Pose(Point(*position), Quaternion(*quaternion)))

def _place(self):
        self.state = "place"
        rospy.loginfo("Placing...")

        place_pose = self.int_markers['place_pose']
        # It seems positions and orientations are randomly required to
        # be actual Point and Quaternion objects or lists/tuples. The
        # least coherent API ever seen.
        self.br.sendTransform(Point2list(place_pose.pose.position),
                              Quaternion2list(place_pose.pose.orientation),
                              rospy.Time.now(),
                              "place_pose",
                              self.robot.base)
        self.robot.place(place_pose)

        # For cubelets:
        place_pose.pose.position.z += 0.042
        # For YCB:
        # place_pose.pose.position.z += 0.026
        self.place_pose = place_pose

def callback_state(self, data):
        for idx, cube in enumerate(data.name):
            self.cubes_state.setdefault(cube, [0] * 3)
            pose = self.cubes_state[cube]
            cube_init = self.CubeMap[cube]["init"]
            pose[0] = data.pose[idx].position.x + cube_init[0]
            pose[1] = data.pose[idx].position.y + cube_init[1]
            pose[2] = data.pose[idx].position.z + cube_init[2]

    # def add_cube(self, name):
    #     p = PoseStamped()
    #     p.header.frame_id = ros_robot.get_planning_frame()
    #     p.header.stamp = rospy.Time.now()
    #
    #     # p.pose = self._arm.get_random_pose().pose
    #     p.pose.position.x = -0.18
    #     p.pose.position.y = 0
    #     p.pose.position.z = 0.046
    #
    #     q = quaternion_from_euler(0.0, 0.0, 0.0)
    #     p.pose.orientation = Quaternion(*q)
    #     ros_scene.add_box(name, p, (0.02, 0.02, 0.02))
    #
    # def remove_cube(self, name):
    #     ros_scene.remove_world_object(name)

def set_cube_pose(self, name, pose, orient=None):
        """
        :param name: cube name, a string
        :param pose: cube position, a list of three float, [x, y, z]
        :param orient: cube orientation, a list of three float, [ix, iy, iz]
        :return:
        """
        self.cubes_pose.link_name = "cubes::" + name
        p = self.cubes_pose.pose
        cube_init = self.CubeMap[name]["init"]
        p.position.x = pose[0] - cube_init[0]
        p.position.y = pose[1] - cube_init[1]
        p.position.z = pose[2] - cube_init[2]
        if orient is None:
            orient = [0, 0, 0]
        q = quaternion_from_euler(orient[0], orient[1], orient[2])
        p.orientation = Quaternion(*q)
        self.pose_pub.publish(self.cubes_pose)

def set_cube_pose(self, name, pose, orient=None):
        """
        :param name: cube name, a string
        :param pose: cube position, a list of three float, [x, y, z]
        :param orient: cube orientation, a list of three float, [ix, iy, iz]
        :return:
        """
        self.cubes_pose.link_name = "cubes::" + name
        p = self.cubes_pose.pose
        p.position.x = pose[0] + 0.18
        p.position.y = pose[1]
        p.position.z = pose[2] - 0.05
        if orient is None:
            orient = [0, 0, 0]
        q = quaternion_from_euler(orient[0], orient[1], orient[2])
        p.orientation = Quaternion(*q)
        self.pose_pub.publish(self.cubes_pose)


# create cube

def _add_table(self, name):
        p = PoseStamped()
        p.header.frame_id = self._robot.get_planning_frame()
        p.header.stamp = rospy.Time.now()

        p.pose.position.x = 0.2
        p.pose.position.y = 0.0
        p.pose.position.z = 0.1

        q = quaternion_from_euler(0.0, 0.0, numpy.deg2rad(90.0))
        p.pose.orientation = Quaternion(*q)

        # Table size from ~/.gazebo/models/table/model.sdf, using the values
        # for the surface link.
        self._scene.add_box(name, p, (0.005, 0.005, 0.005))

        return p.pose

def _add_grasp_block_(self, name):
        p = PoseStamped()
        p.header.frame_id = self._robot.get_planning_frame()
        p.header.stamp = rospy.Time.now()

        # p.pose = self._arm.get_random_pose().pose
        p.pose.position.x = 0.021
        p.pose.position.y = 0.008
        p.pose.position.z = 0.2885

        q = quaternion_from_euler(0.0, 0.0, 0.0)
        p.pose.orientation = Quaternion(*q)

        # Coke can size from ~/.gazebo/models/coke_can/meshes/coke_can.dae,
        # using the measure tape tool from meshlab.
        # The box is the bounding box of the coke cylinder.
        # The values are taken from the cylinder base diameter and height.
        self._scene.add_box(name, p, (0.01, 0.01, 0.01))

        return p.pose

def dict_to_list_euler(pose_dict):

    """
    Convert a pose dictionary to a list in the order x, y, z,
    orientation x, orientation y, orientation z.
    """

    qtn = Quaternion()
    qtn.x = pose_dict['orientation'].x
    qtn.y = pose_dict['orientation'].y
    qtn.z = pose_dict['orientation'].z
    qtn.w = pose_dict['orientation'].w
    elr_x, elr_y, elr_z, = tf.transformations.euler_from_quaternion(
        [qtn.x,qtn.y,qtn.z,qtn.w])

    lst = []
    lst.append(pose_dict['position'].x)
    lst.append(pose_dict['position'].y)
    lst.append(pose_dict['position'].z)
    lst.append(elr_x)
    lst.append(elr_y)
    lst.append(elr_z)
    return lst

def _add_table(self, name):
        p = PoseStamped()
        p.header.frame_id = self._robot.get_planning_frame()
        p.header.stamp = rospy.Time.now()

        p.pose.position.x = 0.45
        p.pose.position.y = 0.0
        p.pose.position.z = 0.22

        q = quaternion_from_euler(0.0, 0.0, numpy.deg2rad(90.0))
        p.pose.orientation = Quaternion(*q)

        # Table size from ~/.gazebo/models/table/model.sdf, using the values
        # for the surface link.
        self._scene.add_box(name, p, (0.5, 0.4, 0.02))

        return p.pose

def _add_grasp_block_(self, name):
        p = PoseStamped()
        p.header.frame_id = self._robot.get_planning_frame()
        p.header.stamp = rospy.Time.now()

        p.pose.position.x = 0.25
        p.pose.position.y = 0.05
        p.pose.position.z = 0.32

        q = quaternion_from_euler(0.0, 0.0, 0.0)
        p.pose.orientation = Quaternion(*q)

        # Coke can size from ~/.gazebo/models/coke_can/meshes/coke_can.dae,
        # using the measure tape tool from meshlab.
        # The box is the bounding box of the coke cylinder.
        # The values are taken from the cylinder base diameter and height.
        self._scene.add_box(name, p, (0.03, 0.03, 0.09))

        return p.pose

def test_transform(self):
        t = Transform(
            translation=Vector3(1, 2, 3),
            rotation=Quaternion(*transformations.quaternion_from_euler(np.pi, 0, 0))
        )

        t_mat = ros_numpy.numpify(t)

        np.testing.assert_allclose(t_mat.dot([0, 0, 1, 1]), [1.0, 2.0, 2.0, 1.0])

        msg = ros_numpy.msgify(Transform, t_mat)

        np.testing.assert_allclose(msg.translation.x, t.translation.x)
        np.testing.assert_allclose(msg.translation.y, t.translation.y)
        np.testing.assert_allclose(msg.translation.z, t.translation.z)
        np.testing.assert_allclose(msg.rotation.x, t.rotation.x)
        np.testing.assert_allclose(msg.rotation.y, t.rotation.y)
        np.testing.assert_allclose(msg.rotation.z, t.rotation.z)
        np.testing.assert_allclose(msg.rotation.w, t.rotation.w)

def test_pose(self):
        t = Pose(
            position=Point(1.0, 2.0, 3.0),
            orientation=Quaternion(*transformations.quaternion_from_euler(np.pi, 0, 0))
        )

        t_mat = ros_numpy.numpify(t)

        np.testing.assert_allclose(t_mat.dot([0, 0, 1, 1]), [1.0, 2.0, 2.0, 1.0])

        msg = ros_numpy.msgify(Pose, t_mat)

        np.testing.assert_allclose(msg.position.x, t.position.x)
        np.testing.assert_allclose(msg.position.y, t.position.y)
        np.testing.assert_allclose(msg.position.z, t.position.z)
        np.testing.assert_allclose(msg.orientation.x, t.orientation.x)
        np.testing.assert_allclose(msg.orientation.y, t.orientation.y)
        np.testing.assert_allclose(msg.orientation.z, t.orientation.z)
        np.testing.assert_allclose(msg.orientation.w, t.orientation.w)

def numpy_to_transform(arr):
    from tf import transformations

    shape, rest = arr.shape[:-2], arr.shape[-2:]
    assert rest == (4,4)

    if len(shape) == 0:
        trans = transformations.translation_from_matrix(arr)
        quat = transformations.quaternion_from_matrix(arr)

        return Transform(
            translation=Vector3(*trans),
            rotation=Quaternion(*quat)
        )
    else:
        res = np.empty(shape, dtype=np.object_)
        for idx in np.ndindex(shape):
            res[idx] = Transform(
                translation=Vector3(*transformations.translation_from_matrix(arr[idx])),
                rotation=Quaternion(*transformations.quaternion_from_matrix(arr[idx]))
            )

def numpy_to_pose(arr):
    from tf import transformations

    shape, rest = arr.shape[:-2], arr.shape[-2:]
    assert rest == (4,4)

    if len(shape) == 0:
        trans = transformations.translation_from_matrix(arr)
        quat = transformations.quaternion_from_matrix(arr)

        return Pose(
            position=Vector3(*trans),
            orientation=Quaternion(*quat)
        )
    else:
        res = np.empty(shape, dtype=np.object_)
        for idx in np.ndindex(shape):
            res[idx] = Pose(
                position=Vector3(*transformations.translation_from_matrix(arr[idx])),
                orientation=Quaternion(*transformations.quaternion_from_matrix(arr[idx]))
            )

def geom_pose_from_rt(rt): 
    msg = geom_msg.Pose()
    msg.position = geom_msg.Point(x=rt.tvec[0], y=rt.tvec[1], z=rt.tvec[2])
    xyzw = rt.quat.to_xyzw()
    msg.orientation = geom_msg.Quaternion(x=xyzw[0], y=xyzw[1], z=xyzw[2], w=xyzw[3])
    return msg

def publish_line_segments(pub_ns, _arr1, _arr2, c='b', stamp=None, frame_id='camera', flip_rb=False, size=0.002): 
    """
    Publish point cloud on:
    pub_ns: Namespace on which the cloud will be published
    arr: numpy array (N x 3) for point cloud data
    c: Option 1: 'c', 'b', 'r' etc colors accepted by matplotlibs color
       Option 2: float ranging from 0 to 1 via matplotlib's jet colormap
       Option 3: numpy array (N x 3) with r,g,b vals ranging from 0 to 1
    s: supported only by matplotlib plotting
    alpha: supported only by matplotlib plotting
    """
    arr1, carr = copy_pointcloud_data(_arr1, c, flip_rb=flip_rb)
    arr2 = (deepcopy(_arr2)).reshape(-1,3)

    marker = vis_msg.Marker(type=vis_msg.Marker.LINE_LIST, ns=pub_ns, action=vis_msg.Marker.ADD)
    if not arr1.shape == arr2.shape: raise AssertionError    

    marker.header.frame_id = frame_id
    marker.header.stamp = stamp if stamp is not None else rospy.Time.now()
    marker.scale.x = size
    marker.scale.y = size
    marker.color.b = 1.0
    marker.color.a = 1.0
    marker.pose.position = geom_msg.Point(0,0,0)
    marker.pose.orientation = geom_msg.Quaternion(0,0,0,1)

    # Handle 3D data: [ndarray or list of ndarrays]
    arr12 = np.hstack([arr1, arr2])
    inds, = np.where(~np.isnan(arr12).any(axis=1))

    marker.points = []
    for j in inds: 
        marker.points.extend([geom_msg.Point(arr1[j,0], arr1[j,1], arr1[j,2]), 
                              geom_msg.Point(arr2[j,0], arr2[j,1], arr2[j,2])])

    # RGB (optionally alpha)
    marker.colors = [std_msg.ColorRGBA(carr[j,0], carr[j,1], carr[j,2], 1.0) 
                     for j in inds]

    marker.lifetime = rospy.Duration()
    _publish_marker(marker)

def publish_pose(pose, stamp=None, frame_id='camera'): 
    msg = geom_msg.PoseStamped();

    msg.header.frame_id = frame_id
    msg.header.stamp = stamp if stamp is not None else rospy.Time.now()    

    tvec = pose.tvec
    x,y,z,w = pose.quat.to_xyzw()
    msg.pose.position = geom_msg.Point(tvec[0],tvec[1],tvec[2])
    msg.pose.orientation = geom_msg.Quaternion(x,y,z,w)

    _publish_pose(msg)

def process_msg(msg, who):
    msg._type == 'nav_msgs/Odometry'

    global last_rear, last_front, last_yaw, last_front_t

    if who == 'rear':
        last_rear = get_position_from_odometry(msg)
    elif who == 'front' and last_rear is not None:
        cur_front = get_position_from_odometry(msg)
        last_yaw = get_yaw(cur_front, last_rear)

        twist_lin = np.dot(rotMatZ(-last_yaw), get_speed_from_odometry(msg))

        if last_front is not None:
            dt = msg.header.stamp.to_sec() - last_front_t
            speed = (cur_front - last_front)/dt
            speed = np.dot(rotMatZ(-last_yaw), speed)
            print '1', speed
            print '2', twist_lin
            print '3', np.sqrt((speed-twist_lin).dot(speed-twist_lin))

            odo = Odometry()
            odo.header.frame_id = '/base_link'
            odo.header.stamp = rospy.Time.now()
            speed_yaw = get_yaw([0,0,0], -speed) #[-speed[0],-speed[1]*100,0])
            speed_yaw_q = ros_tf.transformations.quaternion_from_euler(0, 0, speed_yaw)
            odo.pose.pose.orientation = Quaternion(*list(speed_yaw_q))
            #odo.twist.twist.linear = Point(x=speed[0], y=speed[1], z=speed[2])
            odo.twist.covariance = list(np.eye(6,6).reshape(1,-1).squeeze())
            pub = rospy.Publisher('odo_speed', Odometry, queue_size=1).publish(odo)

        #print last_yaw
        last_front = cur_front
        last_front_t = msg.header.stamp.to_sec()

        return twist_lin

def GetWayPoints(self,Data):
        # dir = os.path.dirname(__file__)
        # filename = dir+'/locationPoint.txt'
        filename = Data.data
        rospy.loginfo(str(filename))
        rospy.loginfo(self.locations)
        self.locations.clear()
        rospy.loginfo(self.locations)

        f = open(filename,'r')

        for i in f.readlines():
            j = i.split(",")
            current_wp_name     = str(j[0])
            rospy.loginfo(current_wp_name)          

            current_point       = Point(float(j[1]),float(j[2]),float(j[3]))
            current_quaternion  = Quaternion(float(j[4]),float(j[5]),float(j[6]),float(j[7]))

            self.locations[current_wp_name] = Pose(current_point,current_quaternion)

        f.close()
        rospy.loginfo(self.locations)

        #Rviz Marker
        self.init_markers()

        self.IsWPListOK = True

def _tuple_to_msg_transform(tf_t):
        """
        Converts a tf tuple into a geometry_msgs/Transform message

        :type tf_t: ((float, float, float), (float, float, float, float))
        :rtype: geometry_msgs.msg.Transform
        """
        transl = Vector3(*tf_t[0])
        rot = Quaternion(*tf_t[1])
        return Transform(transl, rot)

def from_dict(self, in_dict):
        """
        Sets values parsed from a given dictionary.

        :param in_dict: input dictionary.
        :type in_dict: dict[string, string|dict[string,float]]

        :rtype: None
        """
        self.eye_on_hand = in_dict['eye_on_hand']
        self.transformation = TransformStamped(
            child_frame_id=in_dict['tracking_base_frame'],
            transform=Transform(
                Vector3(in_dict['transformation']['x'],
                        in_dict['transformation']['y'],
                        in_dict['transformation']['z']),
                Quaternion(in_dict['transformation']['qx'],
                           in_dict['transformation']['qy'],
                           in_dict['transformation']['qz'],
                           in_dict['transformation']['qw'])
            )
        )
        if self.eye_on_hand:
            self.transformation.header.frame_id = in_dict['robot_effector_frame']
        else:
            self.transformation.header.frame_id = in_dict['robot_base_frame']

def msgToPose(self, detection_pose):
        pose = Pose()

        pose.position.x = detection_pose.x / 1000
        pose.position.y = detection_pose.y / 1000

        if hasattr(detection_pose, 'orientation'):
            quat_tuple = quaternion_from_euler(0.0, 0.0, detection_pose.orientation)
            pose.orientation = Quaternion(*quat_tuple)

        return pose

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