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