Python msg.Pose类代码示例

def particle_to_pose(particle):
    ''' Converts a particle in the form [x, y, theta] into a Pose object '''
    pose = Pose()
    pose.position.x = particle[0]
    pose.position.y = particle[1]
    pose.orientation = angle_to_quaternion(particle[2])
    return pose

def homogeneous_product_pose_transform(pose, transform):
    """
     pose should be Pose() msg
     transform is Transform()
    """
    # 1. Get the transform homogenous matrix.
    Ht = tf.transformations.quaternion_matrix([transform.rotation.x, transform.rotation.y,
                                             transform.rotation.z, transform.rotation.w])

    Tt = [transform.translation.x, transform.translation.y, transform.translation.z]

    Ht[:3,3] = Tt

    # 2.Original pose to homogenous matrix
    H0 = tf.transformations.quaternion_matrix([pose.orientation.x, pose.orientation.y,
                                               pose.orientation.z, pose.orientation.w])

    T0 = [pose.position.x, pose.position.y, pose.position.z]
    H0[:3,3] = T0

    H1 = numpy.dot(H0, Ht)

    # Get results from matrix
    result_position = H1[:3,3].T
    result_position = geometry_msgs.msg.Vector3(result_position[0], result_position[1], result_position[2])
    result_quat     = tf.transformations.quaternion_from_matrix(H1)
    result_quat     = geometry_msgs.msg.Quaternion(result_quat[0], result_quat[1], result_quat[2], result_quat[3])
    result_pose             = Pose()
    result_pose.position    = result_position
    result_pose.orientation = result_quat
    return result_pose

def publisher(paths):
    publisher = rospy.Publisher('pathfinder', Path, queue_size=20)
    rospy.init_node('path_publisher', anonymous=True)
    rate = rospy.Rate(10) # 10 hertz
    msgs=[] #list for all the messages we need to publish
    for path in paths:
        msg = Path() #new path message
        msg.header = create_std_h() #add a header to the path message
        for x,y,z in path:
            #create a pose for each position in the path
            #position is the only field we care about, leave the rest as 0
            ps = PoseStamped()
            ps.header=create_std_h()
            i=Pose()
            i.position=Point(x,y,z)
            ps.pose=i
            msg.poses.append(ps)
        
        msgs.append(msg)
    while not rospy.is_shutdown(): # while we're going
        #publish each message
        for m in msgs:
            #rospy.loginfo(m)
            publisher.publish(m)
        rate.sleep()

    def calculate_mk2_ik(self):

        #goal_point = Point(0.298, -0.249, -0.890) home position

        goal_pose = Pose()

        # Goto position 1
        goal_point = Point(0.298, -0.249, -0.890)
        #goal_point = Point(0.298, -0.5, -1.0)
        goal_pose.position = goal_point
        quat = quaternion_from_euler(0.0, 0.0, 0.0) # roll, pitch, yaw
        goal_pose.orientation = Quaternion(*quat.tolist())
        moveit_goal = self.create_move_group_pose_goal(goal_pose, group="manipulator", end_link_name="link_7", plan_only=False)
        rospy.loginfo("Sending goal...")
        self.moveit_ac.send_goal(moveit_goal)
        rospy.loginfo("Waiting for result...")
        self.moveit_ac.wait_for_result(rospy.Duration(10.0))
        moveit_result = self.moveit_ac.get_result()

        # Goto position 2
        goal_point = Point(0.305, -0.249, -1.05)
        goal_pose.position = goal_point
        quat = quaternion_from_euler(0.0, 0.0, 0.0) # roll, pitch, yaw
        goal_pose.orientation = Quaternion(*quat.tolist())
        moveit_goal = self.create_move_group_pose_goal(goal_pose, group="manipulator", end_link_name="link_7", plan_only=False)
        rospy.loginfo("Sending goal...")
        self.moveit_ac.send_goal(moveit_goal)
        rospy.loginfo("Waiting for result...")
        self.moveit_ac.wait_for_result(rospy.Duration(10.0))
        moveit_result = self.moveit_ac.get_result()

 def execute(self, userdata):
     userdata.orders = [DrinkOrder('david', 'coke'), DrinkOrder('michael', 'milk')]
     pose = Pose()
     pose.position = Point(0.059, 6.26, 0.0)
     pose.orientation = Quaternion(0, 0, -0.59, 0.8)
     userdata.guests_location = pose
     return succeeded

def get_grasp_pose(g_decision_making_state, object_grasp_pose, object_grasp_pose_set):
    
	# load pre-computer grasps
	# loaded_file = numpy.load('../grasp_data/' + g_decision_making_state.object_name + '.npz' )
	
	# loaded_grasp = loaded_file['grasp'][0]
	# print len(loaded_grasp[g_decision_making_state.object_name])

	loaded_file = numpy.load('../grasp_data/' + g_decision_making_state.object_name + '.npz' )
	loaded_grasp = loaded_file['grasp'][0]
	num_of_grasp = len(loaded_grasp[g_decision_making_state.object_name])
	temp = object_grasp_pose
	for i in range(0,num_of_grasp):
		print i
		print_angles(loaded_grasp[g_decision_making_state.object_name][i])
		print_angles(temp)
		object_grasp_pose_temp = temp
		print_angles(temp)
		object_grasp_pose_set[i] = Pose()
		object_grasp_pose_set[i].position = temp.position
		object_grasp_pose_set[i].orientation = rotate_orientation(object_grasp_pose_temp.orientation, loaded_grasp[g_decision_making_state.object_name][i].orientation)

		move_ok = plan_and_move_end_effector_to(object_grasp_pose, 0.3, 
                                            g_bin_positions[g_decision_making_state.bin_id]['map_robot_position_2_ik_seed'])
		print_angles(object_grasp_pose_set[i])
		# raw_input("press Enter to continue...")

	return object_grasp_pose_set

            def obj_response_cb(userdata, response):
                if response.exists:
                    pose = Pose()
                    #Wont there be problems mixing float32 with 64? TODO
                    pose.position = Point(response.base_coordinates.x, response.base_coordinates.y, 0)
                    #TODO, this may give problems
                    pose.orientation = Quaternion(*quaternion_from_euler(0, 0, response.base_coordinates.z))
                    if MANTAIN_DISTANCE_FROM_OBJECT:
                        pose = pose_at_distance(pose, DISTANCE_FROM_OBJECT)
                    userdata.object_location = pose

                    release_pose = ObjectPose()
                    release_pose.pose = response.arm_coordinates
                    userdata.object_release_location = release_pose

                    print "\n\n printing pose for move to object"
                    print pose
                    print "\n\n printing pose for releasing the object"
                    print release_pose
                    print "\n that was the pose of the object\n\n"
                    print userdata.object_name
                    print "is the object name"
                    return succeeded
                else:
                    userdata.object_location = None
                    return aborted

def get_pose_msg_from_2d(x, y, yaw):
    pose = Pose()
    pose.position.x = x
    pose.position.y = y
    pose.position.z = 0
    pose.orientation = Quaternion(*quaternion_from_euler(0,0,yaw))
    return pose

    def publishSphere2(self, pose, color, scale, lifetime=None):
        """
        Publish a sphere Marker. This renders a smoother, flatter-looking sphere.

        @param pose (numpy matrix, numpy ndarray, ROS Pose)
        @param color name (string) or RGB color value (tuple or list)
        @param scale (ROS Vector3, float)
        @param lifetime (float, None = never expire)
        """

        if (self.muted == True):
            return True

        # Convert input pose to a ROS Pose Msg
        if (type(pose) == numpy.matrix) or (type(pose) == numpy.ndarray):
            sphere_pose = mat_to_pose(pose)
        elif type(pose) == Pose:
            sphere_pose = pose
        elif type(pose) == Point:
            pose_msg = Pose()
            pose_msg.position = pose
            sphere_pose = pose_msg
        else:
            rospy.logerr("Pose is unsupported type '%s' in publishSphere()", type(pose).__name__)
            return False

        # Convert input scale to a ROS Vector3 Msg
        if type(scale) == Vector3:
            sphere_scale = scale
        elif type(scale) == float:
            sphere_scale = Vector3(scale, scale, scale)
        else:
            rospy.logerr("Scale is unsupported type '%s' in publishSphere()", type(scale).__name__)
            return False

        # Increment the ID number
        self.sphere_marker.id += 1

        # Get the default parameters
        sphere_marker = self.sphere_marker2 # sphere_marker2 = SPHERE_LIST

        if lifetime == None:
            sphere_marker.lifetime = rospy.Duration(0.0) # 0 = Marker never expires
        else:
            sphere_marker.lifetime = rospy.Duration(lifetime) # in seconds

        # Set the timestamp
        sphere_marker.header.stamp = rospy.Time.now()

        # Set marker size
        sphere_marker.scale = sphere_scale

        # Set marker color
        sphere_marker.color = self.getColor(color)

        # Set the pose of one sphere in the list
        sphere_marker.points[0] = sphere_pose.position
        sphere_marker.colors[0] = self.getColor(color)

        return self.publishMarker(sphere_marker)

def transform_pose(pose_in, transform):
    '''
    Transforms a pose

    Takes a pose defined in the frame defined by transform (i.e. the frame in which transform is the origin) and 
    returns it in the frame in which transform is defined.  Calling this with the origin pose will return transform.
    This returns
        
        pose.point = transform_point(pose_in.point, transform)
        
        pose.orientation = transform_orientation(pose_in.orientation, transform)
    
    **Args:**

        **pose (geometry_msgs.msg.Pose):** Pose to transform

        **transform (geometry_msgs.msg.Pose):** The transform
    
    **Returns:**
        A geometry_msgs.msg.Pose
    '''
    pose = Pose()
    pose.position = transform_point(pose_in.position, transform)
    pose.orientation = transform_quaternion(pose_in.orientation, transform)
    return pose

    def _create_vertices_array(self) :

        for node in self.topo_map.nodes :
            marker = Marker()
            marker.header.frame_id = "/map"
            marker.type = marker.LINE_STRIP
            #marker.lifetime=2
            marker.scale.x = 0.1
            marker.color.a = 0.8
            marker.color.r = 0.7
            marker.color.g = 0.1
            marker.color.b = 0.2
            node._get_coords()
            count=0
            for i in node.vertices :
                #print i[0], i[1]
                Vert = Point()
                Vert.z = 0.05
                Vert.x = node.px + i[0]
                Vert.y = node.py + i[1]
                marker.points.append(Vert)
                vertname = "%s-%d" %(node.name, count)
                Pos = Pose()
                Pos.position = Vert
                self._vertex_marker(vertname, Pos, vertname)
                count+=1
            marker.points.append(marker.points[0])
            self.node_zone.markers.append(marker)
        
        idn = 0
        for m in self.node_zone.markers:
            m.id = idn
            idn += 1 

 def find_joint_pose(self, pose, targ_x=0.0, targ_y=0.0, targ_z=0.0,
                     targ_ox=0.0, targ_oy=0.0, targ_oz=0.0):
     '''
     Finds the joint position of the arm given some pose and the
     offsets from it (to avoid opening the structure all the time
     outside of the function).
     '''
     ik_srv = "ExternalTools/right/PositionKinematicsNode/IKService"
     iksvc = rospy.ServiceProxy(ik_srv, SolvePositionIK)
     ik_request = SolvePositionIKRequest()
     the_pose = deepcopy(pose)
     the_pose['position'] = Point(x=targ_x + self.baxter_off.linear.x,
                                  y=targ_y + self.baxter_off.linear.y,
                                  z=targ_z + self.baxter_off.linear.z)
     angles = tf.transformations.quaternion_from_euler( \
         targ_ox + self.baxter_off.angular.x, \
         targ_oy + self.baxter_off.angular.y, \
         targ_oz + self.baxter_off.angular.z)
     the_pose['orientation'] = Quaternion(x=angles[0],
                                          y=angles[1],
                                          z=angles[2],
                                          w=angles[3])
     approach_pose = Pose()
     approach_pose.position = the_pose['position']
     approach_pose.orientation = the_pose['orientation']
     hdr = Header(stamp=rospy.Time.now(), frame_id='base')
     pose_req = PoseStamped(header=hdr, pose=approach_pose)
     ik_request.pose_stamp.append(pose_req)
     resp = iksvc(ik_request)
     return dict(zip(resp.joints[0].name, resp.joints[0].position))

            def detect_faces_cb_ros_pkg(userdata, status, result):

                '''This code detects faces using the ROS package and then checks the list of people who are not able to walk.
                If the distance of people who are not able to walk is close to the detected faces they are popped from the list.
                First perosn who is closer than ---closestPersonDistance--- are sent to userdata'''

                LOCATION_LIST_DISTANCE = 1
                print "CallBack is called"
                people = result.face_positions
                # pose_in_map = transform_pose(people.pos, people.header.frame_id, "/map")
                if userdata.location_list:
                    for person in people:
                        pose_in_stereo = Pose()
                        pose_in_stereo.position = person.pos

                        pose_in_map = transform_pose(pose_in_stereo, userdata.message.header.frame_id, "/map")

                        for not_walking in userdata.location_list:
                            distance_from_list = ((pose_in_map.position.x - not_walking[0])**2 + (pose_in_map.position.pos.y - not_walking[1])**2)
                            if distance_from_list < LOCATION_LIST_DISTANCE:
                                people.pop(people.index(person))

                closestPerson = None
                closestPersonDistance = sys.maxint
                for person in people:
                    dist = person.pos.z
                    if dist < closestPersonDistance:
                        closestPerson = person
                        closestPersonDistance = dist
                    # FIXME: blacklist already seen people
                userdata.closest_person = closestPerson
                print "Closest person (from find_person_with_visit_checker): ", closestPerson
                return succeeded if closestPerson else aborted

    def __init__(self, from_frame, to_frame, position, orientation):
        self.server = InteractiveMarkerServer("posestamped_im")
        o = orientation
        r, p, y = euler_from_quaternion([o.x, o.y, o.z, o.w])
        rospy.loginfo("Publishing transform and PoseStamped from: " +
                      from_frame + " to " + to_frame +
                      " at " + str(position.x) + " " + str(position.y) + " " + str(position.z) +
                      " and orientation " + str(o.x) + " " + str(o.y) +
                      " " + str(o.z) + " " + str(o.w) + " or rpy " +
                      str(r) + " " + str(p) + " " + str(y))
        self.menu_handler = MenuHandler()
        self.from_frame = from_frame
        self.to_frame = to_frame
        # Transform broadcaster
        self.tf_br = tf.TransformBroadcaster()

        self.pub = rospy.Publisher('/posestamped', PoseStamped, queue_size=1)
        rospy.loginfo("Publishing posestampeds at topic: " + str(self.pub.name))
        pose = Pose()
        pose.position = position
        pose.orientation = orientation
        self.last_pose = pose
        self.print_commands(pose)
        self.makeGraspIM(pose)

        self.server.applyChanges()

    def execute(self, userdata):
        rospy.loginfo("Creating goal to put robot in front of handle")
        pose_handle = Pose()
        if userdata.door_detection_data_in_base_link.handle_side == "left" or userdata.door_detection_data_in_base_link.handle_side == "right":  # closed door
            (r, p, theta) = euler_from_quaternion(( userdata.door_detection_data_in_base_link.door_handle.pose.orientation.x,
                                                    userdata.door_detection_data_in_base_link.door_handle.pose.orientation.y,
                                                    userdata.door_detection_data_in_base_link.door_handle.pose.orientation.z,
                                                    userdata.door_detection_data_in_base_link.door_handle.pose.orientation.w))  # gives back r, p, y
            theta += 3.1416  # the orientation of the door is looking towards the robot, we need the inverse
            pose_handle.orientation = Quaternion(*quaternion_from_euler(0, 0, theta))  # orientation to look parallel to the door
            pose_handle.position.x = userdata.door_detection_data_in_base_link.door_handle.pose.position.x - 0.4  # to align the shoulder with the handle
            pose_handle.position.y = userdata.door_detection_data_in_base_link.door_handle.pose.position.y + 0.2  # refer to upper comment
            pose_handle.position.z = 0.0  # we dont need the Z for moving
            userdata.door_handle_pose_goal = pose_handle
        else:  # open door
            # if it's open... just cross it?
            (r, p, theta) = euler_from_quaternion(( userdata.door_detection_data_in_base_link.door_position.pose.orientation.x,
                                                    userdata.door_detection_data_in_base_link.door_position.pose.orientation.y,
                                                    userdata.door_detection_data_in_base_link.door_position.pose.orientation.z,
                                                    userdata.door_detection_data_in_base_link.door_position.pose.orientation.w))  # gives back r, p, y
            theta += 3.1416  # the orientation of the door is looking towards the robot, we need the inverse
            pose_handle.orientation = Quaternion(*quaternion_from_euler(0, 0, theta))  # orientation to look parallel to the door
            pose_handle.position.x = userdata.door_detection_data_in_base_link.door_position.pose.position.x + 1.0 # enter the door
            pose_handle.position.y = userdata.door_detection_data_in_base_link.door_position.pose.position.y
            userdata.door_handle_pose_goal = pose_handle


        rospy.loginfo("Move base goal: \n" + str(pose_handle))

        return succeeded