hexu/masa-agent
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

bug fixed in auto navigate for ellipse

Browse Source
This commit is contained in:
abnerhexu
2026-01-24 11:05:52 +08:00
parent 26b1a62566
commit 9b05507c5e
4 changed files with 395 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
llm_settings.json
View File

@@ -67,16 +67,16 @@
"system_prompt": "You are a very friendly drone control agent. No matter what language I use to give you instructions, please call the tools to perform the task and then reply in English."
},
"MiniMax": {
"type": "openai-compatible",
"base_url": "https://api.minimaxi.com/v1",
"models_endpoint": "/v1/models",
"chat_endpoint": "/v1/chat/completions",
"type": "anthropic-compatible",
"base_url": "https://api.minimaxi.com/anthropic",
"models_endpoint": "",
"chat_endpoint": "",
"requires_api_key": true,
"api_key": "sk-api-JmFQPSmEc0ErvwEHHJYEkP17TPqpxCx6yK2yS9rU4pBP9eMmdZKPuOo76z8y-bTcA0XsYY6vam33KGM53kQ5GInRCb1clhlNm1Mq-Fq1-CTZQDNXYhYxu7c",
"encoding": "utf-8",
"default_model": "MiniMax-M2.1",
"default_model": "MiniMax-M2",
"default_models": [
"MiniMax-M2.1"
"MiniMax-M2"
],
"allow_endpoint_edit": true,
"allow_api_toggle": true,

33
main.py
View File

@@ -481,15 +481,30 @@ class UAVAgentGUI:
api_key = str(config.get("api_key", "") or "").strip()
# Determine provider type
if provider_type == "ollama":
llm_provider = "ollama"
llm_base_url = None
else:
if "api.openai.com" in base_url:
llm_provider = "openai"
else:
llm_provider = "openai-compatible"
llm_base_url = base_url or None
match provider_type:
case "ollama":
llm_provider = "ollama"
llm_base_url = None
case "openai-compatible":
if "api.openai.com" in base_url:
llm_provider = "openai"
else:
llm_provider = "openai-compatible"
llm_base_url = base_url or None
case "anthropic-compatible":
llm_provider = "anthropic-compatible"
llm_base_url = base_url or None
case _:
raise ValueError(f"Unknown provider type: {provider_type}")
# if provider_type == "ollama":
# llm_provider = "ollama"
# llm_base_url = None
# else:
# if "api.openai.com" in base_url:
# llm_provider = "openai"
# else:
# llm_provider = "openai-compatible"
# llm_base_url = base_url or None
# Check API key requirement
if config.get("requires_api_key") and not api_key:

32
uav_agent.py
View File

@@ -8,6 +8,7 @@ from langchain_classic.agents import AgentExecutor
from langchain_classic.prompts import PromptTemplate
from langchain_ollama import ChatOllama
from langchain_openai import ChatOpenAI
from langchain_anthropic import ChatAnthropic
from langchain_core.outputs import ChatResult
from langchain_core.messages import AIMessage
from uav_api_client import UAVAPIClient
@@ -260,7 +261,7 @@ class UAVControlAgent:
print(f"✅ Ollama LLM initialized")
print()
elif llm_provider in ["openai", "openai-compatible"]:
elif llm_provider in ["openai", "openai-compatible", "anthropic-compatible"]:
if not llm_api_key:
raise ValueError(f"API key is required for {llm_provider} provider. Use --llm-api-key or set environment variable.")
@@ -272,7 +273,7 @@ class UAVControlAgent:
if not llm_base_url:
raise ValueError("llm_base_url is required for openai-compatible provider")
final_base_url = llm_base_url
provider_name = "OpenAI-Compatible API"
provider_name = "Anthropic-Compatible API" if llm_provider == "anthropic-compatible" else "OpenAI-Compatible API"
if self.debug:
print(f" Provider: {provider_name}")
@@ -281,19 +282,22 @@ class UAVControlAgent:
print(f" API Key: {'*' * (len(llm_api_key) - 4) + llm_api_key[-4:] if len(llm_api_key) > 4 else '****'}")
# Create LLM instance
kwargs = {
"model": llm_model,
"temperature": temperature,
"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
if llm_provider == "anthropic-compatible":
kwargs = {
"model_name": llm_model,
"temperature": temperature,
"api_key": llm_api_key,
"base_url": final_base_url
}
# TODO: MiniMax的API对OpenAI的兼容性有点问题。。。
self.llm = ChatOpenAI(**kwargs)
self.llm = ChatAnthropic(**kwargs)
else:
kwargs = {
"model": llm_model,
"temperature": temperature,
"api_key": llm_api_key,
"base_url": final_base_url
}
self.llm = ChatOpenAI(**kwargs)
if self.debug:
print(f"✅ {provider_name} LLM initialized")

356
uav_langchain_tools.py
View File

@@ -494,7 +494,41 @@ def create_uav_tools(client: UAVAPIClient) -> list:
result = client.move_towards(drone_id, distance, heading, dz)
if result["status"] == "error":
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)."
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 to a position where there is no obstacle in that direction, and then move the specified distance along that direction. (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). (3) Try other tools like `move_to` or `auto_navigate_to`"
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\", \"distance\": 10.0}}"
except Exception as e:
return f"Error moving towards: {str(e)}"
@tool
def auto_navigate_move_towards(input_json: str) -> str:
# TODO 能不能把auto navigate和move towards结合起来
"""Navigate the drone to move a drone a specific distance in a direction.
Input should be a JSON string with:
- drone_id: The ID of the drone (required, get from Action list_drones)
- distance: Distance to move in meters (required)
- heading: Heading direction in degrees 0-360 (optional, default: current heading)
- dz: Vertical component in meters (optional)
Example: {{"drone_id": "drone-001", "distance": 10.0, "heading": 90.0}}
"""
try:
params = json.loads(input_json) if isinstance(input_json, str) else input_json
drone_id = params.get('drone_id')
distance = params.get('distance')
heading = params.get('heading')
dz = params.get('dz')
if not drone_id:
return "Error: drone_id is required"
if distance is None:
return "Error: distance is required"
result = client.move_towards(drone_id, distance, heading, dz)
if result["status"] == "error":
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 to a position where there is no obstacle in that direction, and then move the specified distance along that direction. (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). (3) Try other tools like `move_to` or `auto_navigate_to`"
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\", \"distance\": 10.0}}"
@@ -581,7 +615,7 @@ def create_uav_tools(client: UAVAPIClient) -> list:
result = client.move_to(drone_id, x, y, z)
if result["status"] == "error":
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)."
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 to a position where there is no obstacle in that direction, and then move the specified distance along that direction. (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}}"
@@ -589,7 +623,7 @@ def create_uav_tools(client: UAVAPIClient) -> list:
return f"Error moving drone: {str(e)}"
@tool
def navigate_to(input_json: str) -> str:
def auto_navigate_to2(input_json: str) -> str:
"""
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.
@@ -778,7 +812,7 @@ def create_uav_tools(client: UAVAPIClient) -> list:
for obs in mandatory_avoid:
# 这里借用 check_collision 检查点是否在障碍物内 (把线段设为点到点)
# 使用稍小的 buffer 确保节点不紧贴障碍物
collided, _ = GeometryUtils.check_collision(node, node, obs, safety_buffer=1.0)
collided, _ = GeometryUtils.check_collision(node, node, obs, safety_buffer=2.0)
if collided:
is_bad = True
break
@@ -804,7 +838,7 @@ def create_uav_tools(client: UAVAPIClient) -> list:
# 检测边 u-v 是否碰撞 mandatory_avoid
path_blocked = False
for obs in mandatory_avoid:
hit, _ = GeometryUtils.check_collision(u, v, obs, safety_buffer=3.0)
hit, _ = GeometryUtils.check_collision(u, v, obs, safety_buffer=2.0)
if hit:
path_blocked = True
break
@@ -868,7 +902,7 @@ def create_uav_tools(client: UAVAPIClient) -> list:
cruise_alt = max(cruise_alt, sz, tz)
if cruise_alt > drone_max_alt:
return json.dumps({"status": "Failed: Required altitude exceeds drone capability."})
return json.dumps({"status": "Failed: Required altitude exceeds drone capability. Please try finding a path step by step dynamically. Do not repeatedly call optimal_way_to."})
# 6. 构建最终 3D 航点
# 逻辑：
@@ -906,17 +940,321 @@ def create_uav_tools(client: UAVAPIClient) -> list:
if wp == waypoints[0] and len(waypoints) > 1:
pass # 仅作为记录，不发送指令，或者如果这是唯一的点则发送
# 发送指令
# 注意：实际 API 调用中，如果点就是当前位置，可能需要跳过
# 注意：实际 API 调用中，如果点就是当前位置，可能需要跳过（不管了懒得跳了，多调用一次client.move_to()又不花钱）
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 moving to waypoint {wp}: {waypoint_move_result['message']}\nPlease try finding a path dynamically. Do not repeatedly call optimal_way_to."
return f"Error moving to waypoint {wp}: {waypoint_move_result['message']}\nPlease try finding a path step by stepdynamically. Do not repeatedly call optimal_way_to."
return json.dumps({"status": "success", "path": waypoints, "message": waypoint_move_result["message"] + "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."
@tool
def auto_navigate_to(input_json: str) -> str:
"""
Plan an obstacle-avoiding path to the target position (x, y, z), automatically determining whether to detour or overfly.
Fixed: Corrected Ellipse dimension interpretation (width/length are radii, not diameters).
"""
import math
import heapq
import json
# --- 内部几何算法类 ---
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 is_point_in_ellipse(p, center, width, length):
"""
判断点是否在椭圆内。
公式: (x-cx)^2/w^2 + (y-cy)^2/l^2 <= 1
注意：此处 width/length 为半轴长(Radius)
"""
dx = p[0] - center['x']
dy = p[1] - center['y']
return (dx**2 / width**2) + (dy**2 / length**2) <= 1.0
@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':
# [修复]: width 和 length 是半轴长，不是直径，不需要除以2
# 使用最大半轴长作为外接圆半径
r = max(obs.get('width', 0), obs.get('length', 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. 包含检测
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')
tx, ty, tz = params.get('x'), params.get('y'), params.get('z')
if not drone_id or tx is None or ty is None or tz is None:
return "Error: drone_id, x, y, z are required"
# 2. 获取环境信息
status = client.get_drone_status(drone_id)
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 = []
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]
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':
# [修复]: 尺寸不需要除以2
r = max(obs.get('width', 0), obs.get('length', 0))
gen_r = r + safety_margin
# 动态步长：半径越大，采样点越多，防止割圆效应
num_steps = max(8, int(r / 3.0))
for i in range(num_steps):
angle = i * (2 * math.pi / num_steps)
nodes.append((ox + gen_r * math.cos(angle), oy + gen_r * math.sin(angle)))
# 4.2 过滤非法节点
valid_nodes = []
for node in nodes:
is_bad = False
for obs in mandatory_avoid:
# 1. 基础碰撞检测 (外接圆/多边形)
collided, _ = GeometryUtils.check_collision(node, node, obs, safety_buffer=0.5)
if collided:
is_bad = True
break
# 2. 针对椭圆增加精确检测 (防止点落在外接圆内但在椭圆外，或者是很扁的椭圆导致漏判)
# 主要是为了双重保险
if obs['type'] == 'ellipse':
w = obs.get('width', 0)
l = obs.get('length', 0)
# 增加一点 buffer 进行点检测
if GeometryUtils.is_point_in_ellipse(node, obs['position'], w + 0.5, l + 0.5):
is_bad = True
break
if not is_bad:
valid_nodes.append(node)
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 构建图
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]
path_blocked = False
for obs in mandatory_avoid:
hit, _ = GeometryUtils.check_collision(u, v, obs, safety_buffer=2.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 Dijkstra 搜索
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."})
# 5. 高度计算
safe_base_alt = max(sz, tz)
path_max_obs_height = 0.0
for i in range(len(path_2d) - 1):
p1 = path_2d[i]
p2 = path_2d[i+1]
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)
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. 生成航点
waypoints = []
waypoints.append((sx, sy, sz))
if cruise_alt > sz + 0.5:
waypoints.append((sx, sy, cruise_alt))
for i in range(len(path_2d)):
node = path_2d[i]
if i == 0 and math.hypot(node[0]-sx, node[1]-sy) < 0.1:
continue
waypoints.append((node[0], node[1], cruise_alt))
last_wp = waypoints[-1]
if abs(last_wp[2] - tz) > 0.5 or math.hypot(last_wp[0]-tx, last_wp[1]-ty) > 0.1:
waypoints.append((tx, ty, tz))
# 7. 执行
final_msg = "Success"
for wp in waypoints:
# if wp == waypoints[0] and len(waypoints) > 1:
# continue
waypoint_move_result = client.move_to(drone_id, wp[0], wp[1], wp[2])
if waypoint_move_result["status"] == "error":
# 返回详细错误以便调试
return f"Error moving to waypoint {wp}: {waypoint_move_result['message']}"
final_msg = waypoint_move_result.get("message", "Success")
return json.dumps({"status": "success", "path": waypoints, "message": final_msg})
except Exception as e:
return f"Error executing path finding: {str(e)}"
# Return all tools
return [
list_drones,
@@ -931,7 +1269,7 @@ def create_uav_tools(client: UAVAPIClient) -> list:
take_off,
land,
move_to,
navigate_to,
auto_navigate_to,
move_towards,
change_altitude,
hover,