diff --git a/auto_bot_move.py b/auto_bot_move.py index 3e3a89d..3590d23 100644 --- a/auto_bot_move.py +++ b/auto_bot_move.py @@ -305,6 +305,8 @@ class AutoBotMove: self.prepare_route_index = 0 self.prepare_route_completed = not bool(self.prepare_route_waypoints) self.is_running_prepare_route = bool(self.prepare_route_waypoints) + self.prepare_route_snapped = False + self.prepare_route_snap_max_dist = 10.0 layout = load_layout_config() self.patrol_controller = CoordinatePatrol( waypoints, @@ -340,12 +342,52 @@ class AutoBotMove: def _has_prepare_route(self) -> bool: return bool(self.prepare_route_waypoints) + def _snap_prepare_route_to_nearest(self, state): + if self.prepare_route_snapped or self.prepare_route_completed or not self._has_prepare_route(): + return + self.prepare_route_snapped = True + + try: + current_pos = (float(state.get('x')), float(state.get('y'))) + except Exception: + return + + best_idx = None + best_dist = None + for idx, point in enumerate(self.prepare_route_waypoints): + try: + target = (float(point[0]), float(point[1])) + except Exception: + continue + dist = self.patrol_controller.get_distance(current_pos, target) + if best_dist is None or dist < best_dist: + best_idx = idx + best_dist = dist + + if best_idx is None or best_dist is None: + return + if best_dist > self.prepare_route_snap_max_dist: + print( + f">>> [准备路线] 离准备路线最近点仍较远({best_dist:.2f})," + "从第 1 个点开始" + ) + return + + self.prepare_route_index = best_idx + self.patrol_controller.reset_stuck() + print( + f">>> [准备路线] 智能接入最近点 {best_idx + 1}/" + f"{len(self.prepare_route_waypoints)},距离 {best_dist:.2f}" + ) + def _run_prepare_route_step(self, state): if self.prepare_route_completed or not self._has_prepare_route(): self.prepare_route_completed = True self.is_running_prepare_route = False return + self._snap_prepare_route_to_nearest(state) + if self.prepare_route_index >= len(self.prepare_route_waypoints): self.prepare_route_completed = True self.is_running_prepare_route = False