我们从Python开源项目中，提取了以下14个代码示例，用于说明如何使用tf.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 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 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)
