diff --git a/scripts/map_changer b/scripts/map_changer index 10e3792..0232404 100755 --- a/scripts/map_changer +++ b/scripts/map_changer @@ -6,6 +6,7 @@ from std_msgs.msg import UInt16 from nav_msgs.srv import LoadMap from std_srvs.srv import Empty +from std_srvs.srv import Trigger @@ -32,6 +33,8 @@ ## Service clients self.change_map = rospy.ServiceProxy("/change_map", LoadMap) self.amcl_update = rospy.ServiceProxy("/request_nomotion_update", Empty) + self.stop_nav = rospy.ServiceProxy("/stop_wp_nav", Trigger) + self.resume_nav = rospy.ServiceProxy("/resume_nav", Trigger) return @@ -45,6 +48,8 @@ def change_map_service_call(self): + self.stop_nav() + rospy.sleep(0.5) self.update_amcl_call() rospy.wait_for_service("/change_map") try: @@ -52,6 +57,7 @@ if res.result == 0: rospy.loginfo("Successfully changed the map") self.update_amcl_call() + self.resume_nav() return True else: rospy.logerr("Failed to change the map: result=", res.result) @@ -63,7 +69,7 @@ def update_amcl_call(self): rospy.wait_for_service("/request_nomotion_update") - for i in range(0,5): + for i in range(0, 5): self.amcl_update() rospy.sleep(0.5)