optimaze the scan func

template/agent_prompt.py

@@ -91,7 +91,7 @@ Tips for you to finish task in the most efficient way:
 10. Line formation means after finishing the task the two (or more) drones should move to a same position.
 Begin!
 11. Before executing a task or moving to a new position, must get the nearby entities first.
-12. The max moving distance for a single move_to or navigate_to command is 300 meters. If the distance is longer than that, find a mediam point to go first.
+12. The max moving distance for a single move_to or navigate_to command is 500 meters. If the distance is longer than that, find a mediam point to go first.
 13. When targets cannot be found, call `auto_scan_all_environment` as early as possible.
 14. When `auto_scan_all_environment` is not completed, control multiple active drones move to unvisited scan points , use `get_scan_points` to get the list of scan points categorized by visited status and current drone positions.
 15. Executing tasks implicitly performs environment perception, so avoid using the `get_nearby_entities` API as much as possible.

uav_langchain_tools.py

@@ -12,40 +12,6 @@ from typing import List, Dict, Any, Optional
 
 # --- 构建地图scan points ---
 
-# 核心参数
-r = 150  # 侦察半径(m)
-map_width = 1024  # 地图宽度(m)
-map_height = 768  # 地图高度(m)
-dx = r * math.sqrt(3)  # 横向点间距
-dy = 1.5 * r  # 纵向行间距
-
-# 正六边形网格侦察点坐标列表 (x, y)
-drone_points = [
-    # 第1行（奇数行）：y=150.0
-    (round(dx/2, 4), 150.0),          # (129.9038, 150.0)
-    (round(dx/2 + dx, 4), 150.0),    # (389.7114, 150.0)
-    (round(dx/2 + 2*dx, 4), 150.0),  # (649.5190, 150.0)
-    (round(dx/2 + 3*dx, 4), 150.0),  # (909.3266, 150.0)
-    
-    # 第2行（偶数行）：y=375.0 (150+225)
-    (round(dx, 4), 375.0),            # (259.8076, 375.0)
-    (round(dx + dx, 4), 375.0),      # (519.6152, 375.0)
-    (round(dx + 2*dx, 4), 375.0),    # (779.4228, 375.0)
-    (round(dx + 3*dx, 4), 375.0),    # (1039.2304, 375.0)  # 略超1024，覆盖右边缘
-    
-    # 第3行（奇数行）：y=600.0 (375+225)
-    (round(dx/2, 4), 600.0),          # (129.9038, 600.0)
-    (round(dx/2 + dx, 4), 600.0),    # (389.7114, 600.0)
-    (round(dx/2 + 2*dx, 4), 600.0),  # (649.5190, 600.0)
-    (round(dx/2 + 3*dx, 4), 600.0),  # (909.3266, 600.0)
-    
-    # 第4行（偶数行）：y=825.0 (600+225)  # 略超768，覆盖上边缘
-    (round(dx, 4), 825.0),            # (259.8076, 825.0)
-    (round(dx + dx, 4), 825.0),      # (519.6152, 825.0)
-    (round(dx + 2*dx, 4), 825.0),    # (779.4228, 825.0)
-    (round(dx + 3*dx, 4), 825.0)     # (1039.2304, 825.0)
-]
-
 class ScanPointInfo:
     def __init__(self, x: float, y: float):
         self.x: float = x
@@ -60,7 +26,35 @@ class ScanPointInfo:
             "visited": self.visited
         }
 
-scan_points = [ScanPointInfo(x, y) for x, y in drone_points]
+def map_to_drones_points(map_width=1024.0, map_height=768.0) -> List[ScanPointInfo]:
+    """Generate a list of scan points using a hexagonal grid pattern."""
+    # 核心参数
+    r = 150  # 侦察半径(m)
+    dx = r * math.sqrt(3)  # 横向点间距
+    dy = 1.5 * r  # 纵向行间距
+
+    # 确定坐标范围，处理负数、一正一负等情况
+    min_x, max_x = sorted([0.0, float(map_width)])
+    min_y, max_y = sorted([0.0, float(map_height)])
+
+    # 正六边形网格侦察点坐标列表 (x, y)
+    points = []
+    y = min_y + r
+    row = 1
+    while y < max_y + r:
+        # 奇数行偏移 dx/2，偶数行偏移 dx
+        x_start = min_x + ((dx / 2) if row % 2 != 0 else dx)
+        x = x_start
+        while x < max_x + r:
+            points.append(ScanPointInfo(round(x, 4), round(y, 4)))
+            x += dx
+        y += dy
+        row += 1
+    return points
+
+#初始地图
+scan_points = map_to_drones_points(1024,768)
+
 
 def scan_point_is_reached(scan_point: ScanPointInfo, drone_pos: tuple[float, float], threshold: float = 30.0) -> bool:
     """判断当前位置在侦察点附近"""
@@ -1295,6 +1289,10 @@ def create_uav_tools(client: UAVAPIClient) -> list:
                     else:
                         error_str += f" Try not move so far."
                     return json.dumps({"status": "error", "message": error_str})
+                
+                # check if the point is reached by any drone
+                scan_point_is_reached_by_drones(drone_id, wp[0], wp[1])
+
                 final_msg = waypoint_move_result.get("message", "Success")
             
             if len(waypoints) > 1 and move_towards_flag:
@@ -1618,12 +1616,12 @@ def create_uav_tools(client: UAVAPIClient) -> list:
                         tasks[q_drones[i % len(q_drones)]].append(p)
                         assigned_points.add(p)
 
-            print("Drone Quadrants:", drone_quadrants)
+            # print("Drone Quadrants:", drone_quadrants)
-            print("Quadrants Points:", quadrants_points)
+            # print("Quadrants Points:", quadrants_points)
             
             # 兜底：分配那些所在区域没有无人机的点
             remaining_points = [p for p in unvisited_points if p not in assigned_points]
-            print("Remaining points:", remaining_points)
+            # print("Remaining points:", remaining_points)
             if remaining_points:
                 for i, p in enumerate(remaining_points):
                     tasks[active_drones[i % num_drones]].append(p)
@@ -1707,7 +1705,21 @@ def create_uav_tools(client: UAVAPIClient) -> list:
         except Exception as e:
             return f"Error getting scan points: {str(e)}"
 
+    @tool
+    def get_more_possible_scan_points() -> str:
+        """
+        When scan_points are all visited, but cannot find the target, use this function to get more possible scan points, which x or y is negative.
         
+        No input required.
+        """
+        global scan_points
+        scan_points += map_to_drones_points(1024,-768)
+        scan_points += map_to_drones_points(-1024,768)
+        scan_points += map_to_drones_points(-1024,-768)
+        return """SUCCESS: Search area expanded with negative coordinate grids. 
+        ACTION REQUIRED: Call 'get_scan_points' to retrieve the new unvisited points. 
+        STRATEGY: Use `auto_scan_all_environment` first.Then Prioritize unvisited points within a 300m radius of (0,0), like (-157,57), (-157,-256),(256,-157).et.
+        """
     # Return all tools
     return [
         list_drones,
@@ -1740,5 +1752,6 @@ def create_uav_tools(client: UAVAPIClient) -> list:
         get_all_waypoints,
         get_targets,
         auto_scan_all_environment,
-        get_scan_points
+        get_scan_points,
+        get_more_possible_scan_points
     ]