ros python 重置位置

#!/usr/bin/env python
import rospy
import math
import sys
import commands
import yaml
from tf import transformations
from geometry_msgs.msg import PoseWithCovarianceStamped
class PoseSetter(rospy.SubscribeListener):
    def __init__(self, pose):
        self.pose = pose
    def peer_subscribe(self, topic_name, topic_publish, peer_publish):
        p = PoseWithCovarianceStamped()
        p.header.frame_id = "map"
        p.pose.pose.position.x = self.pose[]
        p.pose.pose.position.y = self.pose[]
        (p.pose.pose.orientation.x,
        p.pose.pose.orientation.y,
        p.pose.pose.orientation.z,
        p.pose.pose.orientation.w) = transformations.quaternion_from_euler(, , self.pose[])
        p.pose.covariance[*+] = 0.5 * 0.5
        p.pose.covariance[*+] = 0.5 * 0.5
        p.pose.covariance[*+] = math.pi/12.0 * math.pi/12.0
        peer_publish(p)
        rospy.signal_shutdown("closed!")
filenm="/opt/bp/tmp"
sh="rostopic list|grep initialpose"
globalmap="/opt/bp/maps/global_map.yaml"
x=True
with open(filenm, 'r') as f:
    content=f.read()
    if(content==""):
        with open(globalmap) as k:
            cmapyaml = yaml.load(k)
        with open("/opt/bp/maps/"+str(cmapyaml["hotelid"])+"/"+str(cmapyaml["floor"])+"/map.yaml") as j:
            mapyaml = yaml.load(j)
        pose=mapyaml["origin"]
    else:
        pose =map(float,content.split(","))
while():
    if(commands.getstatusoutput(sh)[]==):
        break
rospy.init_node('pose_setter', anonymous=True)
if(x):
    pub=rospy.Publisher("initialpose", PoseWithCovarianceStamped,PoseSetter(pose),queue_size=)
    x=False
else:
    print("x=False")
rospy.spin()