How to reset odometry after setting /initialpose for amcl
Hello, i'm training a robot for navigation with reinforcement learning in gazebo. After each episode i teleport the robot to a new pose using the function get_free_pose and teleport_robot bellow and then publish the current pose to amcl. Now i need a way to reset the odometry data the ekf_localization node is subscribing to and set it to the current x,y pose that i published to amcl /initialpose. Any Idea how i can do that ?
def get_free_pose(self):
poses = [(0,0),(0.8,0.3),(1.88,0.7),(-1.47,-1.05),(-1.24,-2.08),(-0.5,-2.12),(1.7,-1.5)]
x,y = random.choice(poses)
return x,y
def teleport_robot(self,pose):
model_state_msg = ModelState()
model_state_msg.model_name = "husky"
model_state_msg.pose = pose
model_state_msg.twist = Twist()
model_state_msg.reference_frame = "world"
# Start teleporting in Gazebo
isTeleportSuccess = False
for i in range(5):
if not isTeleportSuccess:
try:
rospy.wait_for_service('/gazebo/set_model_state')
self.telep_model_prox(model_state_msg)
isTeleportSuccess = True
break
except Exception as e:
rospy.logfatal("Error when teleporting agent " + str(e))
else:
rospy.logwarn("Trying to teleporting agent..." + str(i))
time.sleep(2)
if not isTeleportSuccess:
rospy.logfatal("Error when teleporting agent")
return "Err", "Err"`