auto move towards bug fixed

failures.md

@@ -0,0 +1,9 @@
+target assignment large scale 1 
+
+- Drones Drone 3 and Drone 19 form a line formation: both take off to 15 meters. Drone 3 positions at (248, 494, 15) and Drone 19 positions 19 meters behind at (248, 162, 15). Both maintain formation for monitoring. 任务描述有问题，自相矛盾
+
+- Drones Drone 15 and Drone 14 form a line formation: both take off to 16 meters. Drone 15 positions at (522, 538, 16) and Drone 14 positions 24 meters behind at (522, 19, 16). Both maintain formation for monitoring. 任务描述有问题，自相矛盾
+
+- Five-drone coordination exercise: Drone 7, Drone 10, Drone 2, Drone 8, and Drone 20 take off together to 24 meters altitude, then hold formation at positions (435, 516, 24), (557, 331, 24), (790, 445, 24), (663, 685, 24), and (398, 666, 24) respectively for synchronized operation. Then all five drones move north for 89m, and next move east for 92m. Each drone must complete the specified directional movements (north 89m, then east 92m) regardless of starting position - if obstacles block the path, drones should adjust position as needed to complete the full directed distance in each heading. 不清楚drone_has_moved_directed_distance的计算准则，似乎不是叠加的。
+
+- Four-drone target sweep: Drone 14, Drone 7, Drone 18, and Drone 6 take off together to 20 meters altitude. Drone 14 flies to target Fixed Target 8, Drone 7 flies to target Fixed Target 21, Drone 18 flies to target Fixed Target 25, and Drone 6 flies to target Fixed Target 4. After reaching the targets, all four drones move west for 80m, then south for 72m, then north for 56m together. Each drone must complete the specified directional movements (west 80m, south 72m, north 56m) regardless of starting position - if obstacles block the path, drones should adjust position as needed to complete the full directed distance in each heading. 不清楚drone_has_moved_directed_distance的计算准则，似乎不是叠加的。

template/agent_prompt.py

@@ -88,6 +88,7 @@ Tips for you to finish task in the most efficient way:
 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.
 9. Moving X meters in a given direction means reaching a location that is at a distance of X meters from the current point along that direction. The distance must not be reduced due to detouring, nor should the detour proceed in the opposite direction. Obstacle avoidance do not mean you can reduce the distance. Only increase the distance is allowed.
+10. Line formation means after finishing the task the two (or more) drones should move to a same position.
 Begin!
 
 Question: {input}

uav_langchain_tools.py

@@ -740,7 +740,8 @@ def create_uav_tools(client: UAVAPIClient) -> list:
                 "z": final_target_z
             })
 
-            return f"Finding a destination successfully (dist={current_distance}m)... Now use `auto_navigate_to({nav_payload})` to move the drone."
+            # return f"Finding a destination successfully (dist={current_distance}m)... Now use `auto_navigate_to({nav_payload})` to move the drone. This tool safely brings drone to the destination and detour obstacles. First try it with exactly input {nav_payload}, if it fails, then adjust the positions."
+            return auto_navigate_to_non_tool(nav_payload)
             
         except Exception as e:
             return f"Error executing auto_navigate_towards: {str(e)}"
@@ -848,7 +849,10 @@ def create_uav_tools(client: UAVAPIClient) -> list:
         Example: {{"drone_id": "drone-001", "x": 100.0, "y": 50.0, "z": 20.0}}
         """
         # ---------------------------------------------------------
+        result = auto_navigate_to_non_tool(input_json)
+        return result
     
+    def auto_navigate_to_non_tool(input_json: str) -> str:
         try:
             # 1. 解析参数
             params = json.loads(input_json) if isinstance(input_json, str) else input_json
@@ -1230,6 +1234,7 @@ def create_uav_tools(client: UAVAPIClient) -> list:
                         if first_warning == 0:
                             print("Warning: Low battery but no charging stations found!")
                             first_warning = 1
+                            return "Wait for a while and continue calling this function! return [TASK DONE] this time"
                 except Exception as e:
                     print(f"Error during charging process: {str(e)}")