diff --git a/package.xml b/package.xml index 6c2e131..f1e3636 100644 --- a/package.xml +++ b/package.xml @@ -33,10 +33,12 @@ rospy nav_msgs std_msgs + std_srvs rospy nav_msgs std_msgs + std_srvs catkin diff --git a/scripts/map_changer b/scripts/map_changer index 52e0b11..da454c2 100755 --- a/scripts/map_changer +++ b/scripts/map_changer @@ -44,17 +44,13 @@ def change_map_service_call(self): - rospy.wait_for_service("/request_nomotion_update") - res = self.amcl_update() - rospy.sleep(0.5) + self.update_amcl_call() rospy.wait_for_service("/change_map") try: 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") - rospy.wait_for_service("/request_nomotion_update") - res = self.amcl_update() - rospy.sleep(0.5) + self.update_amcl_call() return True else: rospy.logerr("Failed to change the map: result=", res.result) @@ -62,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)