17：无人机远程执行路径规划：A*算法与GPS精准打击

class Node:
    def __init__(self, x, y, parent=None):
        self.x = x
        self.y = y
        self.parent = parent
        self.g = 0  # 从起点到当前节点的代价
        self.h = 0  # 从当前节点到目标节点的估计代价
        self.f = 0  # 总代价
    
    def __eq__(self, other):
        return self.x == other.x and self.y == other.y
    
    def __lt__(self, other):
        return self.f < other.f

import heapq

def astar(grid, start, end):
    """A*算法实现"""
    # 创建起点和终点节点
    start_node = Node(start[0], start[1])
    end_node = Node(end[0], end[1])
    
    # 开放列表和关闭列表
    open_list = []
    closed_list = []
    
    # 将起点加入开放列表
    heapq.heappush(open_list, start_node)
    
    # 定义移动方向
    directions = [(0, -1), (0, 1), (-1, 0), (1, 0), (-1, -1), (-1, 1), (1, -1), (1, 1)]
    
    while open_list:
        # 从开放列表中取出f值最小的节点
        current_node = heapq.heappop(open_list)
        
        # 将当前节点加入关闭列表
        closed_list.append(current_node)
        
        # 检查是否到达终点
        if current_node == end_node:
            path = []
            while current_node:
                path.append((current_node.x, current_node.y))
                current_node = current_node.parent
            return path[::-1]  # 反转路径
        
        # 生成子节点
        for direction in directions:
            # 计算子节点坐标
            new_x = current_node.x + direction[0]
            new_y = current_node.y + direction[1]
            
            # 检查子节点是否在网格内
            if 0 <= new_x < len(grid) and 0 <= new_y < len(grid[0]):
                # 检查子节点是否是障碍物
                if grid[new_x][new_y] == 0:
                    # 创建子节点
                    child_node = Node(new_x, new_y, current_node)
                    
                    # 计算g、h、f值
                    child_node.g = current_node.g + 1
                    # 使用曼哈顿距离作为启发函数
                    child_node.h = abs(child_node.x - end_node.x) + abs(child_node.y - end_node.y)
                    child_node.f = child_node.g + child_node.h
                    
                    # 检查子节点是否在关闭列表中
                    if child_node not in closed_list:
                        # 检查子节点是否在开放列表中
                        in_open = False
                        for node in open_list:
                            if child_node == node and child_node.g >= node.g:
                                in_open = True
                                break
                        
                        if not in_open:
                            heapq.heappush(open_list, child_node)
    
    return None  # 没有找到路径

import serial
import pynmea2

class GPSModule:
    def __init__(self, port="/dev/ttyUSB0", baudrate=9600):
        self.port = port
        self.baudrate = baudrate
        self.ser = None
    
    def connect(self):
        """连接GPS模块"""
        try:
            self.ser = serial.Serial(self.port, self.baudrate, timeout=1)
            return True
        except Exception as e:
            print(f"Failed to connect to GPS module: {e}")
            return False
    
    def get_position(self):
        """获取GPS位置"""
        if not self.ser:
            return None
        
        while True:
            try:
                line = self.ser.readline().decode('utf-8', errors='replace').strip()
                if line.startswith('$GPGGA'):
                    msg = pynmea2.parse(line)
                    if msg.latitude and msg.longitude:
                        return {
                            'latitude': msg.latitude,
                            'longitude': msg.longitude,
                            'altitude': msg.altitude,
                            'timestamp': msg.timestamp
                        }
            except Exception as e:
                print(f"Error reading GPS data: {e}")
                continue
    
    def close(self):
        """关闭连接"""
        if self.ser:
            self.ser.close()

class DGPSModule:
    def __init__(self, base_station_ip, base_station_port):
        self.base_station_ip = base_station_ip
        self.base_station_port = base_station_port
        self.client = None
    
    def connect(self):
        """连接差分GPS基站"""
        try:
            import socket
            self.client = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
            self.client.connect((self.base_station_ip, self.base_station_port))
            return True
        except Exception as e:
            print(f"Failed to connect to DGPS base station: {e}")
            return False
    
    def get_correction_data(self):
        """获取差分修正数据"""
        if not self.client:
            return None
        
        try:
            data = self.client.recv(1024)
            return data
        except Exception as e:
            print(f"Error receiving correction data: {e}")
            return None
    
    def close(self):
        """关闭连接"""
        if self.client:
            self.client.close()

class UAVPathPlanner:
    def __init__(self):
        self.gps = GPSModule()
        self.astar = AStarPlanner()
    
    def plan_path(self, start, end, obstacles):
        """规划路径"""
        # 创建网格地图
        grid = self._create_grid(obstacles)
        
        # 使用A*算法规划路径
        path = self.astar.plan(grid, start, end)
        
        # 路径优化
        optimized_path = self._optimize_path(path)
        
        return optimized_path
    
    def _create_grid(self, obstacles):
        """创建网格地图"""
        # 简化实现，实际应用中需要根据实际环境创建
        grid = [[0 for _ in range(100)] for _ in range(100)]
        
        # 添加障碍物
        for obstacle in obstacles:
            x, y = obstacle
            if 0 <= x < 100 and 0 <= y < 100:
                grid[x][y] = 1
        
        return grid
    
    def _optimize_path(self, path):
        """优化路径"""
        # 简化实现，实际应用中需要更复杂的优化算法
        return path
    
    def execute_mission(self, path):
        """执行任务"""
        for waypoint in path:
            # 飞到 waypoint
            self._fly_to(waypoint)
            
            # 检查是否到达目标
            if waypoint == path[-1]:
                # 执行目标操作
                self._execute_target_operation()
    
    def _fly_to(self, waypoint):
        """飞到指定点"""
        # 简化实现，实际应用中需要更复杂的飞行控制
        print(f"Flying to {waypoint}")
    
    def _execute_target_operation(self):
        """执行目标操作"""
        # 简化实现，实际应用中需要根据任务类型执行不同操作
        print("Executing target operation")

class ObstacleAvoidance:
    def __init__(self, lidar):
        self.lidar = lidar
    
    def detect_obstacles(self):
        """检测障碍物"""
        # 获取激光雷达数据
        data = self.lidar.get_data()
        
        # 处理数据，检测障碍物
        obstacles = []
        for point in data:
            distance = point['distance']
            angle = point['angle']
            
            if distance < 5.0:  # 5米内的障碍物
                # 计算障碍物坐标
                x = distance * math.cos(math.radians(angle))
                y = distance * math.sin(math.radians(angle))
                obstacles.append((x, y))
        
        return obstacles
    
    def adjust_path(self, current_path, obstacles):
        """调整路径以避开障碍物"""
        # 简化实现，实际应用中需要更复杂的路径调整算法
        new_path = current_path.copy()
        
        for obstacle in obstacles:
            # 检查障碍物是否在路径上
            for i, waypoint in enumerate(new_path):
                distance = math.sqrt((waypoint[0] - obstacle[0])**2 + (waypoint[1] - obstacle[1])**2)
                if distance < 2.0:  # 2米内的障碍物
                    # 调整路径
                    new_waypoint = (waypoint[0] + 3.0, waypoint[1] + 3.0)  # 简单偏移
                    new_path[i] = new_waypoint
        
        return new_path

class DynamicPathAdjuster:
    def __init__(self, planner):
        self.planner = planner
    
    def adjust_path(self, current_path, target_position, obstacles):
        """根据目标位置和障碍物动态调整路径"""
        # 获取当前位置
        current_position = self.planner.gps.get_position()
        if not current_position:
            return current_path
        
        # 重新规划路径
        start = (current_position['latitude'], current_position['longitude'])
        end = target_position
        new_path = self.planner.plan_path(start, end, obstacles)
        
        return new_path