localize.py 文件源码

def publish_position(self, pos, ps_pub, ps_cov_pub, cov):
        x, y = pos[0], pos[1]
        if len(pos) > 2:
            z = pos[2]
        else:
            z = 0
        ps = PoseStamped()
        ps_cov = PoseWithCovarianceStamped()
        ps.pose.position.x = x
        ps.pose.position.y = y
        ps.pose.position.z = z
        ps.header.frame_id = self.frame_id
        ps.header.stamp = rospy.get_rostime()
        ps_cov.header = ps.header
        ps_cov.pose.pose = ps.pose
        ps_cov.pose.covariance = cov
        ps_pub.publish(ps)
        ps_cov_pub.publish(ps_cov)