如何用代码设置机器人初始坐标实现 2D Pose Estimate功能

前言：ROS机器人有时候会遇到极端的情况：比如地面打滑严重，IMU精度差，导致积累误差严重，或是amcl匹配错误，导致机器人定位失败，

这时候如何矫正机器人位置变得非常重要，我的思路是利用相机或是在地埋rfid的辅助定位方法。

一、首先是设置机器人初始位置。

主要是发布initalpose这个主题，废话少说直接上代码：

rospy.init_node('test_initalpose', anonymous=False)
    
    rospy.loginfo("start test inital pose...")

    self.setpose_pub = rospy.Publisher("initialpose",PoseWithCovarianceStamped,latch=True, queue_size=1)

    #self.setpose_pub = rospy.Publisher("initialpose", PoseWithCovarianceStamped,queue_size=10)

    self.set_pose = {'x':-4.68,'y':-4.63,'a':1.0}
    self.test_set_pose_flag = True
    self.test_set_pose_cnt = 3


    while self.test_set_pose_flag == True:

        self.set_inital_pose()
        self.test_set_pose_cnt -= 1
        if self.test_set_pose_cnt == 0:
            self.test_set_pose_flag = False
    rospy.sleep(1)

def set_inital_pose(self):
    # Define a set inital pose publisher.
    rospy.loginfo("start set pose...")
    p = PoseWithCovarianceStamped()
    p.header.stamp = rospy.Time.now()
    p.header.frame_id = "map"
    p.pose.pose.position.x = self.set_pose['x']
    p.pose.pose.position.y = self.set_pose['y']
    p.pose.pose.position.z = self.set_pose['a']
    (p.pose.pose.orientation.x,
     p.pose.pose.orientation.y,
     p.pose.pose.orientation.z,
     p.pose.pose.orientation.w) = tf.transformations.quaternion_from_euler(0, 0, self.set_pose['a'])
    p.pose.covariance[6 * 0 + 0] = 0.5 * 0.5
    p.pose.covariance[6 * 1 + 1] = 0.5 * 0.5
    p.pose.covariance[6 * 3 + 3] = math.pi / 12.0 * math.pi / 12.0

    self.setpose_pub.publish(p)

更改self.set_pose的坐标值，如此就可以随意设置机器人坐标了。
有一个比较诡异的事情是，我如果只是发布一次主题，并不能生效，一定需要重复发几次才行，然道ROS会丢失第一次的数据？不明白。因为这个问题困扰了我2天，有知道的朋友请告知，谢谢！