python类transformations()的实例源码

def publish_tf(self,pose, stamp=None):
        """ Publish a tf for the car. This tells ROS where the car is with respect to the map. """
        if stamp == None:
            stamp = rospy.Time.now()

        # this may cause issues with the TF tree. If so, see the below code.
        self.pub_tf.sendTransform((pose[0],pose[1],0),tf.transformations.quaternion_from_euler(0, 0, pose[2]), 
               stamp , "/laser", "/map")

        # also publish odometry to facilitate getting the localization pose
        if self.PUBLISH_ODOM:
            odom = Odometry()
            odom.header = Utils.make_header("/map", stamp)
            odom.pose.pose.position.x = pose[0]
            odom.pose.pose.position.y = pose[1]
            odom.pose.pose.orientation = Utils.angle_to_quaternion(pose[2])
            self.odom_pub.publish(odom)

        return # below this line is disabled

        """
        Our particle filter provides estimates for the "laser" frame
        since that is where our laser range estimates are measure from. Thus,
        We want to publish a "map" -> "laser" transform.

        However, the car's position is measured with respect to the "base_link"
        frame (it is the root of the TF tree). Thus, we should actually define
        a "map" -> "base_link" transform as to not break the TF tree.
        """

        # Get map -> laser transform.
        map_laser_pos = np.array( (pose[0],pose[1],0) )
        map_laser_rotation = np.array( tf.transformations.quaternion_from_euler(0, 0, pose[2]) )
        # Apply laser -> base_link transform to map -> laser transform
        # This gives a map -> base_link transform
        laser_base_link_offset = (0.265, 0, 0)
        map_laser_pos -= np.dot(tf.transformations.quaternion_matrix(tf.transformations.unit_vector(map_laser_rotation))[:3,:3], laser_base_link_offset).T

        # Publish transform
        self.pub_tf.sendTransform(map_laser_pos, map_laser_rotation, stamp , "/base_link", "/map")

def position(self, target_position, trans, height):
        """
        Calculate simple position of the robot's arm.

        Args:
            target_position (Pose): Wanted coordinates of robot's tool
            trans: Calculated transformation
            height (float): Height offset, depends on the number of disks on the rod

        Returns:
            target_position (Pose): Modified coordinates and orientation of robot's tool
        """
        roll = -math.pi / 2
        pitch = 0
        yaw = -math.pi / 2
        quaternion = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
        target_position.orientation.x = quaternion[0]
        target_position.orientation.y = quaternion[1]
        target_position.orientation.z = quaternion[2]
        target_position.orientation.w = quaternion[3]
        target_position.position.x = trans[0]
        target_position.position.y = trans[1]
        target_position.position.z = trans[2] + height
        return target_position

def test_absolute(self):
        """Test robot's ability to position its gripper in absolute coordinates (base frame)."""
        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
            goal.position.x = float(user_input("X?"))
            goal.position.y = float(user_input("Y?"))
            goal.position.z = float(user_input("Z?"))
            goal_final = baxterGoal(id=1, pose=goal)
            self.left_client.send_goal_and_wait(goal_final)
            result = self.left_client.get_result()

def publish_tf(self,pose, stamp=None):
        """ Publish a tf for the car. This tells ROS where the car is with respect to the map. """
        if stamp == None:
            stamp = rospy.Time.now()

        # this may cause issues with the TF tree. If so, see the below code.
        self.pub_tf.sendTransform((pose[0],pose[1],0),tf.transformations.quaternion_from_euler(0, 0, pose[2]), 
               stamp , "/laser", "/map")

        # also publish odometry to facilitate getting the localization pose
        if self.PUBLISH_ODOM:
            odom = Odometry()
            odom.header = Utils.make_header("/map", stamp)
            odom.pose.pose.position.x = pose[0]
            odom.pose.pose.position.y = pose[1]
            odom.pose.pose.orientation = Utils.angle_to_quaternion(pose[2])
            self.odom_pub.publish(odom)

        return # below this line is disabled

        """
        Our particle filter provides estimates for the "laser" frame
        since that is where our laser range estimates are measure from. Thus,
        We want to publish a "map" -> "laser" transform.

        However, the car's position is measured with respect to the "base_link"
        frame (it is the root of the TF tree). Thus, we should actually define
        a "map" -> "base_link" transform as to not break the TF tree.
        """

        # Get map -> laser transform.
        map_laser_pos = np.array( (pose[0],pose[1],0) )
        map_laser_rotation = np.array( tf.transformations.quaternion_from_euler(0, 0, pose[2]) )
        # Apply laser -> base_link transform to map -> laser transform
        # This gives a map -> base_link transform
        laser_base_link_offset = (0.265, 0, 0)
        map_laser_pos -= np.dot(tf.transformations.quaternion_matrix(tf.transformations.unit_vector(map_laser_rotation))[:3,:3], laser_base_link_offset).T

        # Publish transform
        self.pub_tf.sendTransform(map_laser_pos, map_laser_rotation, stamp , "/base_link", "/map")

def get_robot_current_x_y_w(self):
         t = self.tf_listener.getLatestCommonTime(self.base_frame, self.odom_frame)
         position, quaternion = self.tf_listener.lookupTransform(self.odom_frame , self.base_frame,t)

         roll,pitch,yaw = tf.transformations.euler_from_quaternion(quaternion)
#         print 'x = ' ,position[0] ,'y = ', position[1],'yaw =', yaw
         return position[0],position[1],yaw

#????????X?Y?

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 quaternion_to_angle(q):
    """Convert a quaternion _message_ into an angle in radians.
    The angle represents the yaw.
    This is not just the z component of the quaternion."""
    x, y, z, w = q.x, q.y, q.z, q.w
    roll, pitch, yaw = tf.transformations.euler_from_quaternion((x, y, z, w))
    return yaw

def transformations(self):
        """Transform rods' coordinate system to the Baxter's coordinate system."""
        self.listener.waitForTransform("/base", "/rods", rospy.Time(0), rospy.Duration(8.0))
        (trans, rot) = self.listener.lookupTransform('/base', '/rods', rospy.Time(0))
        return trans

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 angle_to_quaternion(angle):
    """Convert an angle in radians into a quaternion _message_."""
    return Quaternion(*tf.transformations.quaternion_from_euler(0, 0, angle))

def quaternion_to_angle(q):
    """Convert a quaternion _message_ into an angle in radians.
    The angle represents the yaw.
    This is not just the z component of the quaternion."""
    x, y, z, w = q.x, q.y, q.z, q.w
    roll, pitch, yaw = tf.transformations.euler_from_quaternion((x, y, z, w))
    return yaw

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 quaternion_to_angle(q):
    """Convert a quaternion _message_ into an angle in radians.
    The angle represents the yaw.
    This is not just the z component of the quaternion."""
    x, y, z, w = q.x, q.y, q.z, q.w
    roll, pitch, yaw = tf.transformations.euler_from_quaternion((x, y, z, w))
    return yaw

def go_to_position(self, task, destination, height, offset_x, offset_y, offset_z):
        """
        Calculate goal position, send that to the robot and wait for response.

        Args:
            task (string): Pick or place action
            destination (int): Destination rod [0, 1, 2]
            height (float): Height of the goal position (based on number of disk on the rod)
            offset_x (float): Offset in robot's x axis
            offset_y (float): Offset in robot's y axis
            offset_z (float): Offset in robot's z axis
        """
        goal = Pose()
        trans = self.transformations()
        if task == 'pick':
            height = get_pick_height(height)
        else:
            height = get_place_height(height)
        goal = self.position(goal, trans, height)

        # Calculate offset from the markers
        if destination == 0: offset_y += self.left_rod_offset
        if destination == 1: offset_y += 0
        if destination == 2: offset_y -= self.right_rod_offset
        offset_x -= self.center_rod_offset

        offset_x -= 0.1   # Moving from rod to rod should be done 10 cm in front of them
        offset_x -= 0.03  # Back up a little to compensate for the width of the disks

        # Update goal with calculated offsets
        goal.position.x += offset_x
        goal.position.y += offset_y
        goal.position.z += offset_z
        goal_final = baxterGoal(id=1, pose=goal)

        # Send goal to Baxter arm server and wait for result
        self.left_client.send_goal_and_wait(goal_final)
        result = self.left_client.get_result()
        if result.status:
            return 1
        else:
            return Errors.RaiseGoToFailed(task, destination, height, offset_x, offset_y, offset_z)