diff --git a/scripts/map_changer b/scripts/map_changer index be0a809..da454c2 100755 --- a/scripts/map_changer +++ b/scripts/map_changer @@ -3,8 +3,9 @@ import numpy as np import ruamel.yaml from pathlib import Path -from nav_msgs.srv import LoadMap from std_msgs.msg import UInt16 +from nav_msgs.srv import LoadMap +from std_srvs.srv import Empty @@ -28,6 +29,8 @@ ## Subscribe current waypoint number self.waypoint_num = 0 self.wp_num_sub = rospy.Subscriber("/waypoint_num", UInt16, self.waypoint_num_callback) + self.change_map = rospy.ServiceProxy("/change_map", LoadMap) + self.amcl_update = rospy.ServiceProxy("/request_nomotion_update", Empty) return @@ -41,12 +44,13 @@ def change_map_service_call(self): + self.update_amcl_call() rospy.wait_for_service("/change_map") try: - change_map = rospy.ServiceProxy("/change_map", LoadMap) - res = change_map(str(self.multimap_dir / Path("map{}.yaml".format(self.current_map_num)))) + res = self.change_map(str(self.multimap_dir / Path("map{}.yaml".format(self.current_map_num)))) if res.result == 0: rospy.loginfo("Successfully changed the map") + self.update_amcl_call() return True else: rospy.logerr("Failed to change the map: result=", res.result) @@ -54,6 +58,13 @@ except rospy.ServiceException: rospy.logerr("Change map service call failed") return False + + + def update_amcl_call(self): + rospy.wait_for_service("/request_nomotion_update") + for i in range(0,5): + self.amcl_update() + rospy.sleep(0.5)