前言：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天，有知道的朋友请告知，谢谢！