from direct.showbase.ShowBaseGlobal import globalClock from direct.task.TaskManagerGlobal import taskMgr from panda3d.core import Point3, Vec3 import math class PatrolSystem: """巡检系统类""" def __init__(self, world): """初始化巡检系统 Args: world: 核心世界对象引用 """ self.world = world # 巡检状态 self.is_patrolling = False self.patrol_points = [] # 巡检点列表 [(pos, hpr, wait_time), ...] self.current_patrol_index = 0 self.patrol_task = None # 巡检参数 self.patrol_speed = 5.0 # 巡检移动速度（单位/秒） self.patrol_turn_speed = 90.0 # 转向速度（度/秒） self.patrol_wait_timer = 0.0 self.patrol_state = "moving" # "moving", "turning_to_target", "waiting", "turning_back" # 相机状态保存 self.original_cam_pos = None self.original_cam_hpr = None print("✓ 巡检系统初始化完成") def add_patrol_point(self, position, heading=None, wait_time=3.0): if heading is None: if self.patrol_points: last_pos = self.patrol_points[-1][0] direction_x = position[0] - last_pos.x direction_y = position[1] - last_pos.y direction_z = position[2] - last_pos.z import math h=math.degrees(math.atan2(-direction_x,-direction_y)) distance_xy = math.sqrt(direction_x**2+direction_y**2) p = math.degrees(math.atan2(direction_z,distance_xy)) p = max(-89,min(89,p)) r=0 heading = (h,p,r) else: # 使用当前相机朝向 current_hpr = self.world.cam.getHpr() heading = (current_hpr.x, current_hpr.y, current_hpr.z) pos = Point3(position[0], position[1], position[2]) hpr = Vec3(heading[0], heading[1], heading[2]) self.patrol_points.append((pos, hpr, wait_time)) print(f"✓ 添加巡检点 {len(self.patrol_points)}: 位置{position}, 朝向{heading}, 停留{wait_time}秒") # 在 PatrolSystem 类中添加以下方法 def add_auto_heading_patrol_point(self, position, wait_time=3.0): """添加自动计算朝向的巡检点（朝向路径前进方向） Args: position: 相机位置 (x, y, z) wait_time: 在该点停留时间（秒） """ heading = None # 将自动计算朝向 # 复用原有的 add_patrol_point 方法 self.add_patrol_point(position, heading, wait_time) def add_patrol_point_looking_at(self, position, look_at_position, wait_time=3.0): """添加朝向指定位置的巡检点 Args: position: 相机位置 (x, y, z) look_at_position: 相机朝向的目标位置 (x, y, z) wait_time: 在该点停留时间（秒） """ import math # 计算从当前位置到目标位置的方向向量 direction_x = look_at_position[0] - position[0] direction_y = look_at_position[1] - position[1] direction_z = look_at_position[2] - position[2] # 计算HPR朝向 h = math.degrees(math.atan2(-direction_x, -direction_y)) distance_xy = math.sqrt(direction_x ** 2 + direction_y ** 2) p = math.degrees(math.atan2(direction_z, distance_xy)) p = max(-89, min(89, p)) # 限制pitch角度在合理范围内 r = 0 # roll通常为0 heading = (h, p, r) self.add_patrol_point(position, heading, wait_time) def clear_patrol_points(self): """清空所有巡检点""" self.patrol_points = [] print("✓ 巡检点已清空") def set_patrol_speed(self, move_speed, turn_speed=None): """设置巡检速度 Args: move_speed: 移动速度（单位/秒） turn_speed: 转向速度（度/秒），如果为None则保持当前值 """ self.patrol_speed = move_speed if turn_speed is not None: self.patrol_turn_speed = turn_speed print(f"✓ 巡检速度已设置: 移动{move_speed}, 转向{turn_speed or self.patrol_turn_speed}") def start_patrol(self): """开始巡检""" if not self.patrol_points: print("✗ 没有设置巡检点，无法开始巡检") return False if self.is_patrolling: print("⚠ 巡检已在进行中") return True # 保存当前相机状态 self.original_cam_pos = Point3(self.world.cam.getPos()) self.original_cam_hpr = Vec3(self.world.cam.getHpr()) # 重置巡检状态 self.current_patrol_index = 0 self.patrol_state = "moving" self.patrol_wait_timer = 0.0 self.is_patrolling = True # 启动巡检任务 if self.patrol_task: taskMgr.remove(self.patrol_task) self.patrol_task = taskMgr.add(self._patrol_task, "patrol_task") print(f"✓ 开始巡检，共{len(self.patrol_points)}个巡检点") return True def stop_patrol(self): """停止巡检""" if not self.is_patrolling: print("⚠ 巡检未在进行中") return False # 停止巡检任务 if self.patrol_task: taskMgr.remove(self.patrol_task) self.patrol_task = None self.is_patrolling = False self.patrol_state = "moving" self.patrol_wait_timer = 0.0 print("✓ 巡检已停止") return True def pause_patrol(self): """暂停巡检""" if not self.is_patrolling: print("⚠ 巡检未在进行中") return False if self.patrol_task: taskMgr.remove(self.patrol_task) self.patrol_task = None print("✓ 巡检已暂停") return True def resume_patrol(self): """恢复巡检""" if self.is_patrolling: print("⚠ 巡检已在进行中") return False if not self.patrol_points: print("✗ 没有设置巡检点") return False self.is_patrolling = True self.patrol_task = taskMgr.add(self._patrol_task, "patrol_task") print("✓ 巡检已恢复") return True def reset_to_original_position(self): """重置相机到原始位置""" if self.original_cam_pos and self.original_cam_hpr: self.world.cam.setPos(self.original_cam_pos) self.world.cam.setHpr(self.original_cam_hpr) print("✓ 相机已重置到原始位置") return True else: print("✗ 没有保存的原始位置") return False def _patrol_task(self, task): """巡检主任务""" try: if not self.is_patrolling or not self.patrol_points: return task.done # 获取当前巡检点 current_point = self.patrol_points[self.current_patrol_index] target_pos, target_hpr, wait_time = current_point # 根据当前状态执行不同操作 if self.patrol_state == "moving": self._handle_moving_state(target_pos) elif self.patrol_state == "turning_to_target": self._handle_turning_to_target_state(target_hpr) elif self.patrol_state == "waiting": self._handle_waiting_state(wait_time) elif self.patrol_state == "turning_back": self._handle_turning_back_state() return task.cont except Exception as e: print(f"巡检任务出错: {e}") import traceback traceback.print_exc() return task.done def _handle_moving_state(self, target_pos): """处理移动状态""" current_pos = self.world.cam.getPos() distance = (target_pos - current_pos).length() if distance < 0.1: # 到达目标点 print(f"✓ 到达巡检点 {self.current_patrol_index + 1}") self.patrol_state = "turning_to_target" return # 计算移动方向和距离 direction = target_pos - current_pos direction.normalize() # 计算目标朝向（看向目标点） target_hpr = self._look_at_to_hpr(direction) current_hpr = self.world.cam.getHpr() # 平滑转向到目标朝向 h_diff = self._normalize_angle(target_hpr.x - current_hpr.x) p_diff = self._normalize_angle(target_hpr.y - current_hpr.y) r_diff = self._normalize_angle(target_hpr.z - current_hpr.z) # 计算本帧应转动的角度 dt = globalClock.getDt() turn_amount = self.patrol_turn_speed * dt # 逐步转向目标角度 new_hpr = Vec3(current_hpr) if abs(h_diff) > turn_amount: new_hpr.x += turn_amount if h_diff > 0 else -turn_amount else: new_hpr.x = target_hpr.x if abs(p_diff) > turn_amount: new_hpr.y += turn_amount if p_diff > 0 else -turn_amount else: new_hpr.y = target_hpr.y if abs(r_diff) > turn_amount: new_hpr.z += turn_amount if r_diff > 0 else -turn_amount else: new_hpr.z = target_hpr.z self.world.cam.setHpr(new_hpr) # 计算本帧应移动的距离 move_distance = self.patrol_speed * dt # 如果移动距离大于剩余距离，则直接移动到目标点 if move_distance >= distance: self.world.cam.setPos(target_pos) else: # 否则按方向移动 new_pos = current_pos + direction * move_distance self.world.cam.setPos(new_pos) def _handle_turning_to_target_state(self, target_hpr): """处理转向目标状态""" # 检查是否需要朝向下一个点 if target_hpr == "look_next": # 计算朝向下一个点的方向 next_index = (self.current_patrol_index + 1) % len(self.patrol_points) next_point_pos = self.patrol_points[next_index][0] current_pos = self.world.cam.getPos() direction = next_point_pos - current_pos direction.normalize() # 计算目标朝向 target_hpr = self._look_at_to_hpr(direction) current_hpr = self.world.cam.getHpr() # 计算角度差 h_diff = self._normalize_angle(target_hpr.x - current_hpr.x) p_diff = self._normalize_angle(target_hpr.y - current_hpr.y) r_diff = self._normalize_angle(target_hpr.z - current_hpr.z) # 检查是否已完成转向 if abs(h_diff) < 1.0 and abs(p_diff) < 1.0 and abs(r_diff) < 1.0: print(f"✓ 完成转向，开始停留") self.patrol_state = "waiting" self.patrol_wait_timer = 0.0 return # 计算本帧应转动的角度 dt = globalClock.getDt() turn_amount = self.patrol_turn_speed * dt # 逐步转向目标角度 new_hpr = Vec3(current_hpr) if abs(h_diff) > turn_amount: new_hpr.x += turn_amount if h_diff > 0 else -turn_amount else: new_hpr.x = target_hpr.x if abs(p_diff) > turn_amount: new_hpr.y += turn_amount if p_diff > 0 else -turn_amount else: new_hpr.y = target_hpr.y if abs(r_diff) > turn_amount: new_hpr.z += turn_amount if r_diff > 0 else -turn_amount else: new_hpr.z = target_hpr.z self.world.cam.setHpr(new_hpr) def _handle_waiting_state(self, wait_time): """处理等待状态""" self.patrol_wait_timer += globalClock.getDt() if self.patrol_wait_timer >= wait_time: print(f"✓ 停留结束，准备转回原朝向") self.patrol_state = "turning_back" # 修改 core/patrol_system.py 中的 _handle_turning_back_state 方法 def _handle_turning_back_state(self): """处理转回原朝向状态""" # 直接完成转向状态，进入移动状态 print(f"✓ 停留结束，开始移动到下一个点") # 移动到下一个巡检点 next_index = (self.current_patrol_index + 1) % len(self.patrol_points) self.current_patrol_index = next_index self.patrol_state = "moving" return def _normalize_angle(self, angle): """规范化角度到-180到180度之间""" while angle > 180: angle -= 360 while angle < -180: angle += 360 return angle def _look_at_to_hpr(self, direction): """将方向向量转换为HPR角度""" # 简化的转换，实际应用中可能需要更精确的计算 h = math.degrees(math.atan2(-direction.x, -direction.y)) p = math.degrees(math.asin(direction.z)) return Vec3(h, p, 0) def get_patrol_status(self): """获取巡检状态信息""" return { "is_patrolling": self.is_patrolling, "current_point": self.current_patrol_index, "total_points": len(self.patrol_points), "state": self.patrol_state, "wait_timer": self.patrol_wait_timer } def list_patrol_points(self): """列出所有巡检点""" if not self.patrol_points: print("没有设置巡检点") return print(f"巡检点列表 (共{len(self.patrol_points)}个):") for i, (pos, hpr, wait_time) in enumerate(self.patrol_points): current_marker = " >>>" if i == self.current_patrol_index and self.is_patrolling else "" print(f" {i + 1}. 位置:({pos.x:.1f}, {pos.y:.1f}, {pos.z:.1f}) " f"朝向:({hpr.x:.1f}, {hpr.y:.1f}, {hpr.z:.1f}) " f"停留:{wait_time}秒{current_marker}") def remove_patrol_point(self, index): """移除指定索引的巡检点""" if 0 <= index < len(self.patrol_points): removed_point = self.patrol_points.pop(index) print( f"✓ 移除巡检点 {index + 1}: 位置({removed_point[0].x:.1f}, {removed_point[0].y:.1f}, {removed_point[0].z:.1f})") # 调整当前索引 if self.current_patrol_index >= len(self.patrol_points) and self.patrol_points: self.current_patrol_index = len(self.patrol_points) - 1 elif self.current_patrol_index >= len(self.patrol_points): self.current_patrol_index = 0 else: print(f"✗ 无效的巡检点索引: {index}") def insert_patrol_point(self, index, position, heading=None, wait_time=3.0): """在指定位置插入巡检点""" if index < 0 or index > len(self.patrol_points): print(f"✗ 无效的插入位置: {index}") return if heading is None: # 使用当前相机朝向 current_hpr = self.world.cam.getHpr() heading = (current_hpr.x, current_hpr.y, current_hpr.z) pos = Point3(position[0], position[1], position[2]) hpr = Vec3(heading[0], heading[1], heading[2]) self.patrol_points.insert(index, (pos, hpr, wait_time)) print(f"✓ 在位置 {index + 1} 插入巡检点: 位置{position}, 朝向{heading}, 停留{wait_time}秒") def update_patrol_point(self, index, position=None, heading=None, wait_time=None): """更新指定巡检点的信息""" if 0 <= index < len(self.patrol_points): pos, hpr, wt = self.patrol_points[index] if position is not None: pos = Point3(position[0], position[1], position[2]) if heading is not None: hpr = Vec3(heading[0], heading[1], heading[2]) if wait_time is not None: wt = wait_time self.patrol_points[index] = (pos, hpr, wt) print(f"✓ 更新巡检点 {index + 1}") else: print(f"✗ 无效的巡检点索引: {index}") def goto_patrol_point(self, index): """直接跳转到指定巡检点""" if not self.patrol_points: print("✗ 没有设置巡检点") return False if 0 <= index < len(self.patrol_points): pos, hpr, _ = self.patrol_points[index] self.world.cam.setPos(pos) self.world.cam.setHpr(hpr) self.current_patrol_index = index print(f"✓ 跳转到巡检点 {index + 1}") return True else: print(f"✗ 无效的巡检点索引: {index}") return False def cleanup(self): """清理巡检系统资源""" self.stop_patrol() self.clear_patrol_points() self.original_cam_pos = None self.original_cam_hpr = None print("✓ 巡检系统资源已清理") # 使用示例和便捷函数 def create_default_patrol_route(patrol_system): """创建默认的巡检路线（示例）""" # 清空现有巡检点 patrol_system.clear_patrol_points() # 添加一些示例巡检点 patrol_system.add_patrol_point((0, -20, 5), (0, -15, 0), 2.0) # 点1：前方低位置 patrol_system.add_patrol_point((0, 0, 10), (0, -30, 0), 3.0) # 点2：中央高位置 patrol_system.add_patrol_point((15, 10, 5), (-45, -10, 0), 2.5) # 点3：右侧位置 patrol_system.add_patrol_point((-15, 10, 5), (45, -10, 0), 2.5) # 点4：左侧位置 print("✓ 默认巡检路线已创建")