552 lines
6.5 KiB
Markdown
552 lines
6.5 KiB
Markdown
补全auto_navigate_move_towards函数，函数的输入包含drone_id，方向和在该方向上的预期移动距离。在遇到障碍物时，需要越过障碍物，并继续移动。最终到达的位置和初始位置相比，在指定的方向上移动的距离应当不小于预期移动距离。
|
||
|
||
@tool
|
||
def auto_navigate_move_towards(input_json: str) -> str:
|
||
try:
|
||
params = json.loads(input_json) if isinstance(input_json, str) else input_json
|
||
pass
|
||
|
||
现在先补全函数，可以调用`get_obstacles`函数获取障碍物信息。该函数返回一个列表。obstacles的种类有4类，是point、circle、polygon、ellipse。这个列表形如：
|
||
|
||
[
|
||
|
||
{
|
||
|
||
"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
|
||
|
||
}
|
||
|
||
]
|
||
|
||
对于高度不为0的obstacles，如果无人机的最大可达高度大于obstacle的高度，就能够飞跃。对于不能飞跃的和高度为0的obstacles，需要绕路。
|
||
|
||
该函数只需为无人机确定一个要到达的目标坐标点。该坐标点需要与障碍物保持一定的距离。确定好目标坐标点后，调用auto_navigate_to函数
|
||
|
||
```
|
||
def auto_navigate_to(input_json: str) -> str:
|
||
"""
|
||
Plan an obstacle-avoiding path using analytic geometry for precise collision detection.
|
||
|
||
Input should be a JSON string with:
|
||
- drone_id: The ID of the drone (required)
|
||
- x: Target X coordinate in meters (required)
|
||
- y: Target Y coordinate in meters (required)
|
||
- z: Target Z coordinate (altitude) in meters (required)
|
||
|
||
Example: {{"drone_id": "drone-001", "x": 100.0, "y": 50.0, "z": 20.0}}
|
||
"""
|
||
```
|
||
|
||
即可将无人机从当前位置导航到目标位置，并避开所有障碍物。
|
||
|
||
现在请补全auto_navigate_move_towards函数。 |