diff --git a/apps/mylib/tools.py b/apps/mylib/tools.py index 50a1545..c1e320a 100644 --- a/apps/mylib/tools.py +++ b/apps/mylib/tools.py @@ -251,7 +251,7 @@ self.selected_layer.map_transparency.set(70) self.map_disp.set_transparency(map_path, 70) self.tra_bar.configure(variable=self.selected_layer.map_transparency) - map_idx += 1 + if "change_map" in point["point"].keys(): map_idx = int(point["point"]["change_map"]) wp_idx += 1 return True @@ -351,12 +351,14 @@ # ベースのfinish poseをウェイポイントに変換 finish_pose: FinishPose = self.map_disp.waypoints_dict[self.map_disp.base_map_key + "fp"] # 個別のウェイポイントをリスト化 + map_idx = 1 wp_yaml_list = [get_waypoint_yaml(waypoints, finish_pose)] wpc = waypoints.get_waypoint(num=1).copy() wpc["x"] = finish_pose.x wpc["y"] = finish_pose.y wpc["stop"] = "true" - wpc["change_map"] = "true" + wpc["change_map"] = map_idx + map_idx += 1 waypoints.append(wpc) # 残りのウェイポイントを追加していく base_map: MyMap = self.map_disp.map_dict[self.map_disp.base_map_key] @@ -397,7 +399,8 @@ wpc["x"] = finish_pose.x wpc["y"] = finish_pose.y wpc["stop"] = "true" - wpc["change_map"] = "true" + wpc["change_map"] = map_idx + map_idx += 1 waypoints.append(wpc) return get_waypoint_yaml(waypoints, finish_pose), wp_yaml_list diff --git a/scripts/map_changer b/scripts/map_changer index f73f36a..ac41b6d 100755 --- a/scripts/map_changer +++ b/scripts/map_changer @@ -20,13 +20,15 @@ self.current_map_num = 0 ## Read waypoints file and change point number self.change_point_num = [] + self.next_map_idx = [] waypoints_path = rospy.get_param("map_changer/waypoints_file") with open(waypoints_path) as file: waypoints_yaml = yaml.load(file) for i, data in enumerate(waypoints_yaml["waypoints"]): - if ("change_map" in data["point"]) and (data["point"]["change_map"]): + if ("change_map" in data["point"]): self.change_point_num.append(i+2) - self.change_point_num = np.array(self.change_point_num, dtype=np.uint16) + self.next_map_idx.append(data["point"]["change_map"]) + # self.change_point_num = np.array(self.change_point_num, dtype=np.uint16) ## Subscribe current waypoint number self.waypoint_num = 0 self.wp_num_sub = rospy.Subscriber("/waypoint_num", UInt16, self.waypoint_num_callback) @@ -40,9 +42,13 @@ def waypoint_num_callback(self, msg): if (self.waypoint_num == msg.data): return - if (np.any(self.change_point_num == msg.data)): - self.current_map_num += 1 + try: + idx = self.change_point_num.index(msg.data) + self.current_map_num = self.next_map_idx[idx] self.change_map_service_call() + except ValueError: + pass + self.waypoint_num = msg.data return