add optimal_way_to using dijkstra

llm_settings.json

@@ -1,5 +1,5 @@
 {
-  "selected_provider": "MiniMax",
+  "selected_provider": "Kimi",
   "provider_configs": {
     "Ollama": {
       "type": "ollama",

paper.md

@@ -0,0 +1,260 @@
+[
+  {
+    "id": "bb07b327",
+    "name": "Point Obstacle 1",
+    "type": "point",
+    "position": {
+      "x": 609.0,
+      "y": 459.0,
+      "z": 0.0
+    },
+    "description": "",
+    "radius": 30.0,
+    "vertices": [],
+    "width": null,
+    "length": null,
+    "height": 0.0,
+    "area": 2827.4333882308138,
+    "created_at": 1766327750.018749,
+    "last_updated": 1766327750.018749
+  },
+  {
+    "id": "fd0760b8",
+    "name": "Point Obstacle 2",
+    "type": "point",
+    "position": {
+      "x": 896.0,
+      "y": 241.0,
+      "z": 0.0
+    },
+    "description": "",
+    "radius": 6.0,
+    "vertices": [],
+    "width": null,
+    "length": null,
+    "height": 0.0,
+    "area": 113.09733552923255,
+    "created_at": 1766327750.0221481,
+    "last_updated": 1766327750.0221481
+  },
+  {
+    "id": "1d817ce2",
+    "name": "Point Obstacle 3",
+    "type": "point",
+    "position": {
+      "x": 809.0,
+      "y": 14.0,
+      "z": 0.0
+    },
+    "description": "",
+    "radius": 8.0,
+    "vertices": [],
+    "width": null,
+    "length": null,
+    "height": 0.0,
+    "area": 201.06192982974676,
+    "created_at": 1766327750.025595,
+    "last_updated": 1766327750.025595
+  },
+  {
+    "id": "2b38acd5",
+    "name": "Circle Obstacle 1",
+    "type": "circle",
+    "position": {
+      "x": 438.0,
+      "y": 465.0,
+      "z": 0.0
+    },
+    "description": "",
+    "radius": 40.0,
+    "vertices": [],
+    "width": null,
+    "length": null,
+    "height": 0.0,
+    "area": 5026.548245743669,
+    "created_at": 1766327750.019903,
+    "last_updated": 1766327750.019903
+  },
+  {
+    "id": "fa67aa30",
+    "name": "Circle Obstacle 2",
+    "type": "circle",
+    "position": {
+      "x": 431.0,
+      "y": 605.0,
+      "z": 0.0
+    },
+    "description": "",
+    "radius": 55.0,
+    "vertices": [],
+    "width": null,
+    "length": null,
+    "height": 0.0,
+    "area": 9503.317777109125,
+    "created_at": 1766327750.0267,
+    "last_updated": 1766327750.0267
+  },
+  {
+    "id": "845b6b36",
+    "name": "Polygon Obstacle 1",
+    "type": "polygon",
+    "position": {
+      "x": 686.0,
+      "y": 669.0,
+      "z": 0.0
+    },
+    "description": "",
+    "radius": null,
+    "vertices": [
+      {
+        "x": 611.0,
+        "y": 594.0
+      },
+      {
+        "x": 761.0,
+        "y": 594.0
+      },
+      {
+        "x": 761.0,
+        "y": 744.0
+      },
+      {
+        "x": 611.0,
+        "y": 744.0
+      }
+    ],
+    "width": null,
+    "length": null,
+    "height": 0.0,
+    "area": 22500.0,
+    "created_at": 1766327750.021056,
+    "last_updated": 1766327750.021056
+  },
+  {
+    "id": "8603630e",
+    "name": "Polygon Obstacle 2",
+    "type": "polygon",
+    "position": {
+      "x": 312.0,
+      "y": 232.0,
+      "z": 0.0
+    },
+    "description": "",
+    "radius": null,
+    "vertices": [
+      {
+        "x": 247.0,
+        "y": 167.0
+      },
+      {
+        "x": 377.0,
+        "y": 167.0
+      },
+      {
+        "x": 377.0,
+        "y": 297.0
+      },
+      {
+        "x": 247.0,
+        "y": 297.0
+      }
+    ],
+    "width": null,
+    "length": null,
+    "height": 0.0,
+    "area": 16900.0,
+    "created_at": 1766327750.023377,
+    "last_updated": 1766327750.023377
+  },
+  {
+    "id": "b32c509b",
+    "name": "Polygon Obstacle 3",
+    "type": "polygon",
+    "position": {
+      "x": 691.0,
+      "y": 344.0,
+      "z": 0.0
+    },
+    "description": "",
+    "radius": null,
+    "vertices": [
+      {
+        "x": 625.0,
+        "y": 278.0
+      },
+      {
+        "x": 757.0,
+        "y": 278.0
+      },
+      {
+        "x": 757.0,
+        "y": 410.0
+      },
+      {
+        "x": 625.0,
+        "y": 410.0
+      }
+    ],
+    "width": null,
+    "length": null,
+    "height": 0.0,
+    "area": 17424.0,
+    "created_at": 1766327750.027871,
+    "last_updated": 1766327750.027871
+  },
+  {
+    "id": "b6a5ec7c",
+    "name": "Polygon Obstacle 4",
+    "type": "polygon",
+    "position": {
+      "x": 237.0,
+      "y": 555.0,
+      "z": 0.0
+    },
+    "description": "",
+    "radius": null,
+    "vertices": [
+      {
+        "x": 181.0,
+        "y": 499.0
+      },
+      {
+        "x": 293.0,
+        "y": 499.0
+      },
+      {
+        "x": 293.0,
+        "y": 611.0
+      },
+      {
+        "x": 181.0,
+        "y": 611.0
+      }
+    ],
+    "width": null,
+    "length": null,
+    "height": 0.0,
+    "area": 12544.0,
+    "created_at": 1766327750.0289862,
+    "last_updated": 1766327750.0289862
+  },
+  {
+    "id": "9bf135c3",
+    "name": "Ellipse Obstacle 1",
+    "type": "ellipse",
+    "position": {
+      "x": 475.0,
+      "y": 156.0,
+      "z": 0.0
+    },
+    "description": "",
+    "radius": null,
+    "vertices": [],
+    "width": 46.0,
+    "length": 34.0,
+    "height": 0.0,
+    "area": 4913.450910214437,
+    "created_at": 1766327750.0245,
+    "last_updated": 1766327750.0245
+  }
+]

template/agent_prompt.py

@@ -86,6 +86,7 @@ Tips for you to finish task in the most efficient way:
 5. If battery is below 30, find the nerest waypoint, go there and land, then charge to 100.
 6. Reaching to a higher latitude can help you see targets, but do not exceed the drone's limit.
 7. Cannot move from current status: DroneStatus.IDLE means you need to take off first then move.
+8. If the start point is different with the current position of the drone, first go to the start point, then continue the left paths.
 
 Begin!
 

template/parsing_error.py

@@ -15,6 +15,6 @@ REMINDER - Action Input must be valid JSON:
 - For multiple parameters: {{"drone_id": "drone-001", "altitude": 15.0}}
 - Numbers WITHOUT quotes, strings WITH quotes
 - We put your answer to langchain, so if you want to return {{ or }}, return double of the characters.
-- When the task is done, output "Final Answer:\n[TASK DONE]"
+- When the task is done, simply output "Final Answer:\n[TASK DONE]"
 
 Please try again with proper JSON format."""

uav_agent.py

@@ -263,7 +263,12 @@ class UAVControlAgent:
                 "api_key": llm_api_key,
                 "base_url": final_base_url
             }
-
+            if llm_model == "MiniMax-M2.1":
+                reasoning = {
+                    "effort": "low",  # 'low', 'medium', or 'high'
+                    "summary": "auto",  # 'detailed', 'auto', or None
+                }
+                # TODO: MiniMax的API对OpenAI的兼容性有点问题。。。
             self.llm = ChatOpenAI(**kwargs)
 
             if self.debug:

uav_langchain_tools.py

@@ -579,17 +579,19 @@ def create_uav_tools(client: UAVAPIClient) -> list:
 
             result = client.move_to(drone_id, x, y, z)
             if result["status"] == "error":
-                result["message"] += " If the task is to move a certain distance in a specific direction but the path is blocked by an obstacle, first move a certain distance in another direction within the plane, and then proceed to move in the target direction. Do not alter the intended distance of movement in the target direction unless explicitly permitted by the task. You may invoke `get_obstacles` to observe the positions of obstacles."
+                result["message"] += "(1) If the task is to move a certain distance in a specific direction but the path is blocked by an obstacle, first move a certain distance in another direction within the plane, and then proceed to move in the target direction. Do not alter the intended distance of movement in the target direction unless explicitly permitted by the task. You may invoke `get_obstacles` to observe the positions of obstacles. (2) If the obstacle’s height is lower than the maximum altitude the drone can reach, the drone may ascend to an altitude higher than the obstacle and fly over it. If the obstacle's height is 0, then it indicates no drone can fly over it (In this case you need to detour)."
             return json.dumps(result, indent=2)
         except json.JSONDecodeError as e:
             return f"Error parsing JSON input: {str(e)}. Expected format: {{\"drone_id\": \"drone-001\", \"x\": 100.0, \"y\": 50.0, \"z\": 20.0}}"
         except Exception as e:
             return f"Error moving drone: {str(e)}"
-    
+
     @tool
     def optimal_way_to(input_json: str) -> str:
-        """Get the optimal path to a specific 3D coordinates (x, y, z).
+        """
-        Always check for collisions first using check_path_collision.
+        Plan an obstacle-avoiding path to the target position (x, y, z), automatically determining whether to detour or overfly:
+        1. Detour around obstacles that cannot be overflown—such as those exceeding the drone’s maximum operational altitude or located within no-fly zones.
+        2. For obstacles that can be safely overflown, automatically compute a safe flight altitude that exceeds the obstacle’s height while remaining within the drone’s operational ceiling, and incorporate this into the vertical profile of the trajectory.
 
         Input should be a JSON string with:
         - drone_id: The ID of the drone (required)
@@ -599,25 +601,319 @@ def create_uav_tools(client: UAVAPIClient) -> list:
 
         Example: {{"drone_id": "drone-001", "x": 100.0, "y": 50.0, "z": 20.0}}
         """
+        import math
+        import heapq
+        
+        # --- 内部几何算法类 (封装以保持主逻辑清晰) ---
+        class GeometryUtils:
+            @staticmethod
+            def dist_sq(p1, p2):
+                return (p1[0]-p2[0])**2 + (p1[1]-p2[1])**2
+
+            @staticmethod
+            def point_to_segment_dist(p, a, b):
+                """计算点 p 到线段 ab 的最短距离"""
+                px, py = p[0], p[1]
+                ax, ay = a[0], a[1]
+                bx, by = b[0], b[1]
+                
+                l2 = (ax - bx)**2 + (ay - by)**2
+                if l2 == 0: return math.hypot(px - ax, py - ay)
+                
+                t = ((px - ax) * (bx - ax) + (py - ay) * (by - ay)) / l2
+                t = max(0, min(1, t))
+                
+                proj_x = ax + t * (bx - ax)
+                proj_y = ay + t * (by - ay)
+                return math.hypot(px - proj_x, py - proj_y)
+
+            @staticmethod
+            def segments_intersect(a1, a2, b1, b2):
+                """判断线段 a1-a2 与 b1-b2 是否相交"""
+                def ccw(A, B, C):
+                    return (C[1]-A[1]) * (B[0]-A[0]) > (B[1]-A[1]) * (C[0]-A[0])
+                return ccw(a1, b1, b2) != ccw(a2, b1, b2) and ccw(a1, a2, b1) != ccw(a1, a2, b2)
+
+            @staticmethod
+            def is_point_in_polygon(p, vertices):
+                """射线法判断点是否在多边形内"""
+                x, y = p[0], p[1]
+                inside = False
+                j = len(vertices) - 1
+                for i in range(len(vertices)):
+                    xi, yi = vertices[i]['x'], vertices[i]['y']
+                    xj, yj = vertices[j]['x'], vertices[j]['y']
+                    
+                    intersect = ((yi > y) != (yj > y)) and \
+                                (x < (xj - xi) * (y - yi) / (yj - yi + 1e-9) + xi)
+                    if intersect:
+                        inside = not inside
+                    j = i
+                return inside
+
+            @staticmethod
+            def check_collision(p1, p2, obs, safety_buffer=2.0):
+                """
+                检测线段 p1-p2 是否与障碍物 obs 碰撞。
+                返回: (Boolean 是否碰撞, Float 障碍物高度)
+                """
+                otype = obs['type']
+                opos = obs['position']
+                ox, oy = opos['x'], opos['y']
+                obs_height = obs.get('height', 0)
+
+                # 1. 圆形/椭圆/点 (简化为圆)
+                if otype in ['circle', 'point', 'ellipse']:
+                    if otype == 'ellipse':
+                        r = max(obs.get('width', 0), obs.get('length', 0)) / 2.0
+                    else:
+                        r = obs.get('radius', 0)
+                    
+                    limit = r + safety_buffer
+                    dist = GeometryUtils.point_to_segment_dist((ox, oy), p1, p2)
+                    if dist < limit:
+                        return True, obs_height
+
+                # 2. 多边形
+                elif otype == 'polygon':
+                    verts = obs['vertices']
+                    if not verts: return False, 0
+                    
+                    # A. 边对边相交检测
+                    for i in range(len(verts)):
+                        v1 = (verts[i]['x'], verts[i]['y'])
+                        v2 = (verts[(i + 1) % len(verts)]['x'], verts[(i + 1) % len(verts)]['y'])
+                        if GeometryUtils.segments_intersect(p1, p2, v1, v2):
+                            return True, obs_height
+                    
+                    # B. 包含检测 (起点或终点在多边形内)
+                    # 注意：这里不加 safety_buffer 进行包含检测，因为如果点在内部必撞
+                    # 若要处理 buffer，需要做多边形 offset，这里略过复杂操作，依赖边相交检测
+                    if GeometryUtils.is_point_in_polygon(p1, verts) or GeometryUtils.is_point_in_polygon(p2, verts):
+                        return True, obs_height
+                
+                return False, 0
+
+        # ---------------------------------------------------------
+        
         try:
+            # 1. 解析参数
             params = json.loads(input_json) if isinstance(input_json, str) else input_json
             drone_id = params.get('drone_id')
-            x = params.get('x')
+            tx, ty, tz = params.get('x'), params.get('y'), params.get('z')
-            y = params.get('y')
-            z = params.get('z')
 
-            if not drone_id:
+            if not drone_id or tx is None or ty is None or tz is None:
-                return "Error: drone_id is required"
+                return "Error: drone_id, x, y, z are required"
-            if x is None or y is None or z is None:
-                return "Error: x, y, and z coordinates are required"
             
-            result = client.optimal_way_to(drone_id, x, y, z)
+            direct_move_result = client.move_to(drone_id, tx, ty, tz)
-            return json.dumps(result, indent=2)
+            if direct_move_result["status"] != "error":
-        except json.JSONDecodeError as e:
+                return json.dumps(direct_move_result, indent=2)
-            return f"Error parsing JSON input: {str(e)}. Expected format: {{\"drone_id\": \"drone-001\", \"x\": 100.0, \"y\": 50.0, \"z\": 20.0}}"
+            # 2. 获取环境信息
-        except Exception as e:
+            status = client.get_drone_status(drone_id)
-            return f"Error moving drone: {str(e)}"
+            start_pos = status['position']
+            sx, sy, sz = start_pos['x'], start_pos['y'], start_pos['z']
+            drone_max_alt = status.get('max_altitude', 100.0)
+            
+            all_obstacles = client.get_obstacles()
 
+            # 3. 障碍物分类
+            # mandatory_avoid: 必须绕路 (高度 >= 无人机极限 或 高度为0的禁飞区)
+            # fly_over_candidates: 可能可以飞跃 (高度 < 无人机极限)
+            mandatory_avoid = []
+            fly_over_candidates = []
+
+            for obs in all_obstacles:
+                h = obs.get('height', 0)
+                if h == 0 or h >= drone_max_alt:
+                    mandatory_avoid.append(obs)
+                else:
+                    fly_over_candidates.append(obs)
+
+            target_point = (tx, ty)
+            start_point = (sx, sy)
+
+            # 4. 2D 路径规划 (仅避开 mandatory_avoid)
+            
+            # 4.1 生成节点
+            # 包括起点、终点
+            nodes = [start_point, target_point]
+            
+            # 为必须绕行的障碍物生成关键点 (外扩 5m)
+            safety_margin = 5.0
+            for obs in mandatory_avoid:
+                opos = obs['position']
+                ox, oy = opos['x'], opos['y']
+                
+                if obs['type'] == 'polygon':
+                    # 简单策略：取多边形顶点并向外延伸
+                    # 更稳健的方法：不做复杂几何外扩，直接取顶点，但在碰撞检测时留余量
+                    # 这里为了路径稀疏性，取顶点 + 向量外扩
+                    center_x = sum(v['x'] for v in obs['vertices']) / len(obs['vertices'])
+                    center_y = sum(v['y'] for v in obs['vertices']) / len(obs['vertices'])
+                    for v in obs['vertices']:
+                        vx, vy = v['x'], v['y']
+                        vec_len = math.hypot(vx - center_x, vy - center_y)
+                        if vec_len > 0:
+                            scale = (vec_len + safety_margin) / vec_len
+                            nx = center_x + (vx - center_x) * scale
+                            ny = center_y + (vy - center_y) * scale
+                            nodes.append((nx, ny))
+                else:
+                    # 圆/椭圆/点
+                    r = obs.get('radius', 0)
+                    if obs['type'] == 'ellipse':
+                        r = max(obs.get('width', 0), obs.get('length', 0)) / 2.0
+                    r += safety_margin
+                    nodes.append((ox + r, oy))
+                    nodes.append((ox - r, oy))
+                    nodes.append((ox, oy + r))
+                    nodes.append((ox, oy - r))
+
+            # 4.2 过滤非法节点 (落入其他障碍物内的节点)
+            valid_nodes = []
+            for node in nodes:
+                is_bad = False
+                for obs in mandatory_avoid:
+                    # 这里借用 check_collision 检查点是否在障碍物内 (把线段设为点到点)
+                    # 使用稍小的 buffer 确保节点不紧贴障碍物
+                    collided, _ = GeometryUtils.check_collision(node, node, obs, safety_buffer=1.0)
+                    if collided:
+                        is_bad = True
+                        break
+                if not is_bad:
+                    valid_nodes.append(node)
+            
+            # 确保起点和终点在 valid_nodes 中 (如果起点就在禁飞区，这里会抛出无解，符合逻辑)
+            # 为防止浮点误差导致起点被过滤，强制加回起点终点(如果它们真的在障碍物内，后面边的检测会挡住)
+            if start_point not in valid_nodes: valid_nodes.insert(0, start_point)
+            if target_point not in valid_nodes: valid_nodes.append(target_point)
+            
+            # 重新映射索引
+            start_idx = valid_nodes.index(start_point)
+            target_idx = valid_nodes.index(target_point)
+            
+            # 4.3 构建图 (Dijkstra)
+            # 只检测 mandatory_avoid
+            adj = {i: [] for i in range(len(valid_nodes))}
+            for i in range(len(valid_nodes)):
+                for j in range(i + 1, len(valid_nodes)):
+                    u, v = valid_nodes[i], valid_nodes[j]
+                    
+                    # 检测边 u-v 是否碰撞 mandatory_avoid
+                    path_blocked = False
+                    for obs in mandatory_avoid:
+                        hit, _ = GeometryUtils.check_collision(u, v, obs, safety_buffer=3.0)
+                        if hit:
+                            path_blocked = True
+                            break
+                    
+                    if not path_blocked:
+                        dist = math.hypot(u[0]-v[0], u[1]-v[1])
+                        adj[i].append((j, dist))
+                        adj[j].append((i, dist))
+
+            # 4.4 搜索最短 2D 路径
+            pq = [(0.0, start_idx, [valid_nodes[start_idx]])]
+            visited = set()
+            path_2d = []
+            
+            while pq:
+                cost, u, path = heapq.heappop(pq)
+                if u == target_idx:
+                    path_2d = path
+                    break
+                if u in visited: continue
+                visited.add(u)
+                
+                for v_idx, w in adj[u]:
+                    if v_idx not in visited:
+                        heapq.heappush(pq, (cost + w, v_idx, path + [valid_nodes[v_idx]]))
+            
+            if not path_2d:
+                return json.dumps({"status": "Failed: No 2D path found. Target might be in restricted area."})
+
+            # 5. 路径高度计算 (Path Refinement)
+            # 我们现在有了一条避开了“不可飞越障碍物”的 2D 路径。
+            # 现在需要沿着这条路径，检查它是否穿过了“可飞越障碍物”，并计算所需高度。
+            
+            # 基础安全高度：起点高度、目标高度、以及基本的最低飞行高度
+            safe_base_alt = max(sz, tz) 
+            
+            # 我们需要分段处理，因为路径不同路段可能经过不同高度的障碍物
+            # 简单策略：计算整条路径所需的全局最大安全高度 (Global Safe Altitude for this path)
+            # 优化策略：虽然可以做分段高度，但在实际无人机操作中，保持平稳的一个巡航高度通常更安全且节能
+            
+            path_max_obs_height = 0.0
+            
+            for i in range(len(path_2d) - 1):
+                p1 = path_2d[i]
+                p2 = path_2d[i+1]
+                
+                # 检查此路段与所有 fly_over_candidates 的碰撞情况
+                for obs in fly_over_candidates:
+                    hit, obs_h = GeometryUtils.check_collision(p1, p2, obs, safety_buffer=2.0)
+                    if hit:
+                        # 如果撞到了这个可飞越障碍物，我们需要比它高
+                        path_max_obs_height = max(path_max_obs_height, obs_h)
+
+            # 最终决定的飞行高度 (加 2米 安全余量)
+            if path_max_obs_height > 0:
+                cruise_alt = path_max_obs_height + 2.0
+            else:
+                cruise_alt = safe_base_alt
+
+            # 确保不低于起点和终点，且不超过最大限高
+            cruise_alt = max(cruise_alt, sz, tz)
+            
+            if cruise_alt > drone_max_alt:
+                return json.dumps({"status": "Failed: Required altitude exceeds drone capability."})
+
+            # 6. 构建最终 3D 航点
+            # 逻辑：
+            # 1. 如果需要爬升，先在当前位置 (sx, sy) 爬升到 cruise_alt
+            # 2. 平飞经过所有 2D 路径点 (高度 = cruise_alt)
+            # 3. 最后如果在目标点上方，下降/调整到 tz
+
+            waypoints = []
+            
+            # 起点
+            current_waypoint = (sx, sy, sz)
+            waypoints.append(current_waypoint)
+            
+            # 爬升阶段 (如果巡航高度高于当前高度)
+            if cruise_alt > sz + 0.5: # 0.5 作为浮点容差
+                waypoints.append((sx, sy, cruise_alt))
+            
+            # 巡航阶段 (中间节点)
+            # 跳过 path_2d[0] 因为它是起点
+            for i in range(len(path_2d)):
+                node = path_2d[i]
+                waypoints.append((node[0], node[1], cruise_alt))
+                
+            # 接近终点阶段
+            # 先平飞到终点上方
+            waypoints.append((tx, ty, cruise_alt))
+            
+            # 如果巡航高度与目标高度不同，最后调整高度
+            if abs(cruise_alt - tz) > 0.5:
+                waypoints.append((tx, ty, tz))
+                
+            # 7. 执行飞行
+            for wp in waypoints:
+                # 跳过与当前位置极近的点 (避免重复指令)
+                if wp == waypoints[0] and len(waypoints) > 1:
+                    pass # 仅作为记录，不发送指令，或者如果这是唯一的点则发送
+                # 发送指令
+                # 注意：实际 API 调用中，如果点就是当前位置，可能需要跳过
+                waypoint_move_result = client.move_to(drone_id, wp[0], wp[1], wp[2])
+                if waypoint_move_result["status"] == "error":
+                    print(f"Error moving to waypoint {wp}: {waypoint_move_result['message']}")
+                    return f"Error executing path finding: {str(e)}\nPlease try finding a path dynamically. Do not repeatedly call optimal_way_to."
+                
+            return json.dumps({"status": "success", "path": waypoints, "info": "You can call get_drone_status to check the drone's current status."})
+
+        except Exception as e:
+            return f"Error executing path finding: {str(e)}\nPlease try finding a path dynamically."
 
     # Return all tools
     return [
@@ -633,7 +929,7 @@ def create_uav_tools(client: UAVAPIClient) -> list:
         take_off,
         land,
         move_to,
-        # optimal_way_to,
+        optimal_way_to,
         move_towards,
         change_altitude,
         hover,