Python msg.PointStamped类代码示例

    def publishPosition(self, position):
        msg = PointStamped()
        msg.header.stamp = rospy.Time.now()
        msg.header.frame_id = self.frame

        msg.point = Point(position[0], position[1], 0)
        self.publisher.publish(msg)

def to_msg_vector(vector):
    msg = PointStamped()
    msg.header = vector.header
    msg.point.x = vector[0]
    msg.point.y = vector[1]
    msg.point.z = vector[2]
    return msg

    def face_callback(self, msg):
        if not self.found_face:
            face = PointStamped()
            face.header = msg.people[0].header
            face.point = msg.people[0].pos
            self.face_parent_frame = msg.people[0].header.frame_id
            # self.listener.waitForTransform(face.header.frame_id, 'base_link', rospy.Time.now(), rospy.Duration(5.0))
            d = sqrt(face.point.x * face.point.x + face.point.y * face.point.y)
       
            # Change the axes from camera-type axes 
            self.quaternion = quaternion_from_euler(pi/2, pi/2, 0.0)
            pose = PoseStamped()
            pose.header = face.header
            pose.pose.position = face.point
            pose.pose.orientation.x = self.quaternion[0]
            pose.pose.orientation.y = self.quaternion[1]
            pose.pose.orientation.z = self.quaternion[2]
            pose.pose.orientation.w = self.quaternion[3]

            # Transform to base_link
            # pose = self.listener.transformPose('base_link', pose)
            face = pose.pose.position
            self.quaternion = (pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w)


            self.origin = (face.x, face.y, face.z)

            # Flip the bit
            self.found_face = True

    def move(self, group, target="", pose=None):

        assert target != "" or pose is not None

        # group.set_workspace([minX, minY, minZ, maxX, maxY, maxZ])

        if pose is not None:
            group.set_pose_target(pose)
            pt = PointStamped()
            pt.header = pose.header
            pt.point = pose.pose.position
            self.look_at_cb(pt)
        else:

            group.set_named_target(target)



        group.allow_looking(False)
        group.allow_replanning(False)
        group.set_num_planning_attempts(1)

        cnt = 3

        while cnt > 0:

            if group.go(wait=True):
                return True

            rospy.sleep(1)

        return False

def create_point_stamped(point, frame_id = 'base_link'):
    m = PointStamped()
    m.header.frame_id = frame_id
    m.header.stamp = rospy.Time()
    #m.header.stamp = rospy.get_rostime()
    m.point = Point(*point)
    return m

def pre_load():
 rospy.loginfo('检测到预注册的位置')
 rospy.loginfo('读取预设位置')
 count=getpass.getuser()
 read=open('/home/%s/xu_slam/src/nav_staff/map/pre_regist_pose.txt'%count,'r')
 pose=read.readlines()
 read.close()
 poses=eval(pose[0])
 try:
  intial=rospy.wait_for_message("odom",Odometry)
  intial_point=PointStamped()
  intial_point.point=intial.pose.pose.position
  intial_point.header.stamp=rospy.Time.now()
  intial_point.header.frame_id='map'
 except:
  pass
 pose_list=[]
 for i in range(len(poses)):
  default_point=PointStamped()
  default_point.header.frame_id='map'
  default_point.header.stamp=rospy.Time.now()
  default_point.point.x=poses['pose_%s'%i]['x']
  default_point.point.y=poses['pose_%s'%i]['y']
  default_point.point.z=poses['pose_%s'%i]['z']
  default_point.header.seq=i+1
  pose_list.append(default_point)
 pose_list.append(intial_point)
 tasks(len(pose_list),pose_list)

 def new_place_position(self):
     current_pose = utils.manipulation.get_arm_move_group().get_current_pose()
     destination = PointStamped()
     destination.header.frame_id = current_pose.header.frame_id
     destination.point = current_pose.pose.position
     destination.point.z = 0
     return destination

    def convertStereo(self, u, v, disparity):
        """
        Converts two pixel coordinates u and v along with the disparity to give PointStamped
        
        This code was taken from stereo_click package
        """

        if not self.canProcess():
            return None
        
        for key in self.locks.keys():
            self.locks[key].acquire()

        stereoModel = image_geometry.StereoCameraModel()
        stereoModel.fromCameraInfo(self.leftInfo, self.rightInfo)
        (x,y,z) = stereoModel.projectPixelTo3d((u,v), disparity)
        
        cameraPoint = PointStamped()
        cameraPoint.header.frame_id = self.leftInfo.header.frame_id
        cameraPoint.header.stamp = rospy.Time.now()
        cameraPoint.point = Point(x,y,z)

        self.listener.waitForTransform(self.outputFrame, cameraPoint.header.frame_id, rospy.Time.now(), rospy.Duration(4.0))
        outputPoint = self.listener.transformPoint(self.outputFrame, cameraPoint)

        for key in self.locks.keys():
            self.locks[key].release()

        return outputPoint

def main():
  
    rospy.init_node('pr2_state_machine')
    brain = PR2RobotBrain()
    brain.getReady()
    
    area_to_explore = PolygonStamped()
    center_point = PointStamped()
    
    now = rospy.Time.now()
    
    area_to_explore.header.stamp = now
    area_to_explore.header.frame_id = "map"
    points = [Point32(-1.65, -1.56, 0.0),
              Point32(5.41, -1.7, 0.0),
              Point32(5.57, 4.44, 0.0),
              Point32(-1.75, 4.37, 0.0)]
              
    area_to_explore.polygon = Polygon(points)
    
    center_point.header = area_to_explore.header
    center_point.point.x = 1.83
    center_point.point.y = 1.57
    center_point.point.z = 0.0
    
    brain = PR2RobotBrain(area_to_explore, center_point)
    brain.loop()

    def pose_cb(self, pose):
        m_pose = PointStamped()
        m_pose.header = pose.header
        m_pose.point = pose.pose.position
        m_pose = self.listener.transformPoint(self.reference_frame,m_pose)
        self.goal_x = m_pose.point.x
        self.goal_y = m_pose.point.y
        print "New goal: %.2f %.2f" % (self.goal_x, self.goal_y)

        marker = Marker()
        marker.header = pose.header
        marker.ns = "goal_marker"
        marker.id = 10
        marker.type = Marker.CYLINDER
        marker.action = Marker.ADD
        marker.pose = pose.pose
        marker.scale.x = 0.1
        marker.scale.y = 0.1
        marker.scale.z = 0.5;
        marker.color.a = 1.0;
        marker.color.r = 1.0;
        marker.color.g = 1.0;
        marker.color.b = 0.0;
        marker.lifetime.secs=-1.0
        self.marker_pub.publish(marker)

def euroc_object_to_odom_combined(euroc_object):
    header = Header(0, rospy.Time(0), euroc_object.frame_id)

    # Convert the centroid
    camera_point = PointStamped()
    camera_point.header = header
    camera_point.point = euroc_object.c_centroid
    odom_point = manipulation.transform_to(camera_point, '/odom_combined')
    euroc_object.c_centroid = odom_point.point
    euroc_object.frame_id = '/odom_combined'

    # Convert the cuboid
    if euroc_object.c_cuboid_success:
        cuboid_posestamped = PoseStamped(header, euroc_object.object.primitive_poses[0])
        cuboid_posestamped = manipulation.transform_to(cuboid_posestamped, '/odom_combined')
        euroc_object.object.primitive_poses[0] = cuboid_posestamped.pose
        euroc_object.object.header.frame_id = '/odom_combined'

    # Convert the mpe_object
    if euroc_object.mpe_success:
        for i in range(0, len(euroc_object.mpe_object.primitive_poses)):
            posestamped = PoseStamped(header, euroc_object.mpe_object.primitive_poses[i])
            posestamped = manipulation.transform_to(posestamped, '/odom_combined')
            euroc_object.mpe_object.primitive_poses[i] = posestamped.pose
            euroc_object.mpe_object.header.frame_id = '/odom_combined'

	def runFilter(self):

		r = rospy.Rate(self.filterRate) 
		while not rospy.is_shutdown():
			if self.flag_reset:
				self.kf.reset(self.getReset())
				self.flag_reset = 0

			self.kf.iteration(self.getMeasures())

			self.pose_pub_.publish(self.kf.getState())
			
			person_point = PointStamped()
			person_point.header = self.kf.getState().header
			person_point.header.stamp = rospy.Time.now()
			person_point.point = self.kf.getState().pose.position
			self.point_pub_.publish(person_point)

			self.tf_person.sendTransform((self.kf.getState().pose.position.x,self.kf.getState().pose.position.y,0),
                     		(self.kf.getState().pose.orientation.x,self.kf.getState().pose.orientation.y,self.kf.getState().pose.orientation.z,self.kf.getState().pose.orientation.w),
                     		rospy.Time.now(),
                     		"person_link",
                     		self.kf.getState().header.frame_id)

			r.sleep()

def estimation(msg):
    uwbdis = msg.distance

    point = PointStamped()
    point.header = Header()
    point.header.frame_id = "vicon"
    point.header.stamp = rospy.Time.now()
    point.point = msg.position

 
    listener.waitForTransform("ned", "vicon", rospy.Time.now(), rospy.Duration(0.05))
    point_tf = listener.transformPoint("ned", point)
    uwbanchor = array([point_tf.point.x, point_tf.point.y, point_tf.point.z])

    imp = array(msg.point)
    global q,a,r,xe
    
    #xe, _ = uwb.locate(xe, Q, 1.0/100, uwbdis, uwbanchor, q, a, r)
    xe, _ = vision.locate(xe, Q, 1.0/100, imp, visionanchor, q, a, r)
 
    
    x_msg = state()
    x_msg.state.header = Header()
    x_msg.state.header.frame_id = "ned"
    x_msg.state.header.stamp = rospy.Time.now()
    x_msg.state.pose.position = Point(xe[0], xe[1], xe[2])
    x_msg.state.pose.orientation = Quaternion(xe[3], xe[4], xe[5], xe[6])
    x_msg.velocity = Vector3(xe[7], xe[8], xe[9])
    x_msg.bias = xe[10]
    pub.publish(x_msg)

def people_cb(meas):
    global mouth_center
    ps = PointStamped()
    ps.point = meas.pos
    ps.header = meas.header
    ps.header.stamp = rospy.Time(0)
    point = tfl.transformPoint(model.tf_frame, ps)
    mouth_center = model.project3dToPixel((point.point.x, point.point.y + mouth_offset, point.point.z))

def do_transform_point(point, transform):
    p = transform_to_kdl(transform) * PyKDL.Vector(point.point.x, point.point.y, point.point.z)
    res = PointStamped()
    res.point.x = p[0]
    res.point.y = p[1]
    res.point.z = p[2]
    res.header = transform.header
    return res