可基本执行任务
This commit is contained in:
ywb
+1
-5
@@ -29,7 +29,7 @@ ARM_CONFIG = {
|
|||||||
|
|
||||||
# ========== 地图 ==========
|
# ========== 地图 ==========
|
||||||
MAP_CONFIG = {
|
MAP_CONFIG = {
|
||||||
"map_dir": "/home/elephant/agv_pro_ros2/install/agv_pro_navigation2/share/agv_pro_navigation2/map/",
|
"map_dir": "/home/elephant/agv_pro_ros2/src/agv_pro_navigation2/map/",
|
||||||
"map_file": "map.yaml",
|
"map_file": "map.yaml",
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -85,10 +85,6 @@ class State:
|
|||||||
RUNNING = "running"
|
RUNNING = "running"
|
||||||
PAUSED = "paused"
|
PAUSED = "paused"
|
||||||
IDLE = "idle"
|
IDLE = "idle"
|
||||||
WAITING_ERROR = "waiting_error"
|
|
||||||
WAITING_STEP = "waiting_step"
|
|
||||||
WAITING_ERROR = "waiting_error"
|
|
||||||
WAITING_STEP = "waiting_step"
|
|
||||||
|
|
||||||
class PhotoType:
|
class PhotoType:
|
||||||
FRONT = "front"
|
FRONT = "front"
|
||||||
|
|||||||
+162
-189
@@ -1,46 +1,4 @@
|
|||||||
[
|
[
|
||||||
{
|
|
||||||
"id": "m_0_2",
|
|
||||||
"row": 0,
|
|
||||||
"col": 2,
|
|
||||||
"front": {
|
|
||||||
"coords": [
|
|
||||||
1.2421705407118802,
|
|
||||||
0.0025490140048510445,
|
|
||||||
0.00150923641
|
|
||||||
],
|
|
||||||
"poses": []
|
|
||||||
},
|
|
||||||
"back": {
|
|
||||||
"coords": [
|
|
||||||
0,
|
|
||||||
0,
|
|
||||||
0
|
|
||||||
],
|
|
||||||
"poses": []
|
|
||||||
}
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"id": "m_1_2",
|
|
||||||
"row": 1,
|
|
||||||
"col": 2,
|
|
||||||
"front": {
|
|
||||||
"coords": [
|
|
||||||
0,
|
|
||||||
0,
|
|
||||||
0
|
|
||||||
],
|
|
||||||
"poses": []
|
|
||||||
},
|
|
||||||
"back": {
|
|
||||||
"coords": [
|
|
||||||
0,
|
|
||||||
0,
|
|
||||||
0
|
|
||||||
],
|
|
||||||
"poses": []
|
|
||||||
}
|
|
||||||
},
|
|
||||||
{
|
{
|
||||||
"id": "m_2_0",
|
"id": "m_2_0",
|
||||||
"row": 2,
|
"row": 2,
|
||||||
@@ -209,111 +167,6 @@
|
|||||||
"poses": []
|
"poses": []
|
||||||
}
|
}
|
||||||
},
|
},
|
||||||
{
|
|
||||||
"id": "m_0_1",
|
|
||||||
"row": 0,
|
|
||||||
"col": 1,
|
|
||||||
"front": {
|
|
||||||
"coords": [
|
|
||||||
0,
|
|
||||||
0,
|
|
||||||
0
|
|
||||||
],
|
|
||||||
"poses": []
|
|
||||||
},
|
|
||||||
"back": {
|
|
||||||
"coords": [
|
|
||||||
0,
|
|
||||||
0,
|
|
||||||
0
|
|
||||||
],
|
|
||||||
"poses": []
|
|
||||||
}
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"id": "m_0_0",
|
|
||||||
"row": 0,
|
|
||||||
"col": 0,
|
|
||||||
"front": {
|
|
||||||
"coords": [
|
|
||||||
0,
|
|
||||||
0,
|
|
||||||
0
|
|
||||||
],
|
|
||||||
"poses": []
|
|
||||||
},
|
|
||||||
"back": {
|
|
||||||
"coords": [
|
|
||||||
0,
|
|
||||||
0,
|
|
||||||
0
|
|
||||||
],
|
|
||||||
"poses": []
|
|
||||||
}
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"id": "m_1_1",
|
|
||||||
"row": 1,
|
|
||||||
"col": 1,
|
|
||||||
"front": {
|
|
||||||
"coords": [
|
|
||||||
0,
|
|
||||||
0,
|
|
||||||
0
|
|
||||||
],
|
|
||||||
"poses": []
|
|
||||||
},
|
|
||||||
"back": {
|
|
||||||
"coords": [
|
|
||||||
0,
|
|
||||||
0,
|
|
||||||
0
|
|
||||||
],
|
|
||||||
"poses": []
|
|
||||||
}
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"id": "m_1_0",
|
|
||||||
"row": 1,
|
|
||||||
"col": 0,
|
|
||||||
"front": {
|
|
||||||
"coords": [
|
|
||||||
0,
|
|
||||||
0,
|
|
||||||
0
|
|
||||||
],
|
|
||||||
"poses": []
|
|
||||||
},
|
|
||||||
"back": {
|
|
||||||
"coords": [
|
|
||||||
0,
|
|
||||||
0,
|
|
||||||
0
|
|
||||||
],
|
|
||||||
"poses": []
|
|
||||||
}
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"id": "m_0_3",
|
|
||||||
"row": 0,
|
|
||||||
"col": 3,
|
|
||||||
"front": {
|
|
||||||
"coords": [
|
|
||||||
0,
|
|
||||||
0,
|
|
||||||
0
|
|
||||||
],
|
|
||||||
"poses": []
|
|
||||||
},
|
|
||||||
"back": {
|
|
||||||
"coords": [
|
|
||||||
0,
|
|
||||||
0,
|
|
||||||
0
|
|
||||||
],
|
|
||||||
"poses": []
|
|
||||||
}
|
|
||||||
},
|
|
||||||
{
|
{
|
||||||
"id": "m_0_4",
|
"id": "m_0_4",
|
||||||
"row": 0,
|
"row": 0,
|
||||||
@@ -356,27 +209,6 @@
|
|||||||
"poses": []
|
"poses": []
|
||||||
}
|
}
|
||||||
},
|
},
|
||||||
{
|
|
||||||
"id": "m_1_3",
|
|
||||||
"row": 1,
|
|
||||||
"col": 3,
|
|
||||||
"front": {
|
|
||||||
"coords": [
|
|
||||||
0,
|
|
||||||
0,
|
|
||||||
0
|
|
||||||
],
|
|
||||||
"poses": []
|
|
||||||
},
|
|
||||||
"back": {
|
|
||||||
"coords": [
|
|
||||||
0,
|
|
||||||
0,
|
|
||||||
0
|
|
||||||
],
|
|
||||||
"poses": []
|
|
||||||
}
|
|
||||||
},
|
|
||||||
{
|
{
|
||||||
"id": "m_1_5",
|
"id": "m_1_5",
|
||||||
"row": 1,
|
"row": 1,
|
||||||
@@ -398,27 +230,6 @@
|
|||||||
"poses": []
|
"poses": []
|
||||||
}
|
}
|
||||||
},
|
},
|
||||||
{
|
|
||||||
"id": "m_1_4",
|
|
||||||
"row": 1,
|
|
||||||
"col": 4,
|
|
||||||
"front": {
|
|
||||||
"coords": [
|
|
||||||
0,
|
|
||||||
0,
|
|
||||||
0
|
|
||||||
],
|
|
||||||
"poses": []
|
|
||||||
},
|
|
||||||
"back": {
|
|
||||||
"coords": [
|
|
||||||
0,
|
|
||||||
0,
|
|
||||||
0
|
|
||||||
],
|
|
||||||
"poses": []
|
|
||||||
}
|
|
||||||
},
|
|
||||||
{
|
{
|
||||||
"id": "m_2_3",
|
"id": "m_2_3",
|
||||||
"row": 2,
|
"row": 2,
|
||||||
@@ -502,5 +313,167 @@
|
|||||||
],
|
],
|
||||||
"poses": []
|
"poses": []
|
||||||
}
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"id": "m_0_0",
|
||||||
|
"row": 0,
|
||||||
|
"col": 0,
|
||||||
|
"front": {
|
||||||
|
"coords": [
|
||||||
|
0,
|
||||||
|
0,
|
||||||
|
0
|
||||||
|
],
|
||||||
|
"poses": []
|
||||||
|
},
|
||||||
|
"back": {
|
||||||
|
"coords": [
|
||||||
|
0,
|
||||||
|
0,
|
||||||
|
0
|
||||||
|
],
|
||||||
|
"poses": []
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"id": "m_1_0",
|
||||||
|
"row": 1,
|
||||||
|
"col": 0,
|
||||||
|
"front": {
|
||||||
|
"coords": [
|
||||||
|
0,
|
||||||
|
0,
|
||||||
|
0
|
||||||
|
],
|
||||||
|
"poses": []
|
||||||
|
},
|
||||||
|
"back": {
|
||||||
|
"coords": [
|
||||||
|
0,
|
||||||
|
0,
|
||||||
|
0
|
||||||
|
],
|
||||||
|
"poses": []
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"id": "m_0_1",
|
||||||
|
"row": 0,
|
||||||
|
"col": 1,
|
||||||
|
"front": {
|
||||||
|
"coords": [
|
||||||
|
0,
|
||||||
|
0,
|
||||||
|
0
|
||||||
|
],
|
||||||
|
"poses": []
|
||||||
|
},
|
||||||
|
"back": {
|
||||||
|
"coords": [
|
||||||
|
0,
|
||||||
|
0,
|
||||||
|
0
|
||||||
|
],
|
||||||
|
"poses": []
|
||||||
|
},
|
||||||
|
"qr": {
|
||||||
|
"coords": [
|
||||||
|
0,
|
||||||
|
0,
|
||||||
|
0
|
||||||
|
],
|
||||||
|
"qr_value": "",
|
||||||
|
"model_id": ""
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"id": "m_0_2",
|
||||||
|
"row": 0,
|
||||||
|
"col": 2,
|
||||||
|
"front": {
|
||||||
|
"coords": [
|
||||||
|
0,
|
||||||
|
0,
|
||||||
|
0
|
||||||
|
],
|
||||||
|
"poses": []
|
||||||
|
},
|
||||||
|
"back": {
|
||||||
|
"coords": [
|
||||||
|
0,
|
||||||
|
0,
|
||||||
|
0
|
||||||
|
],
|
||||||
|
"poses": []
|
||||||
|
},
|
||||||
|
"qr": {
|
||||||
|
"coords": [
|
||||||
|
0,
|
||||||
|
0,
|
||||||
|
0
|
||||||
|
],
|
||||||
|
"qr_value": "",
|
||||||
|
"model_id": ""
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"id": "m_0_3",
|
||||||
|
"row": 0,
|
||||||
|
"col": 3,
|
||||||
|
"front": {
|
||||||
|
"coords": [
|
||||||
|
0,
|
||||||
|
0,
|
||||||
|
0
|
||||||
|
],
|
||||||
|
"poses": []
|
||||||
|
},
|
||||||
|
"back": {
|
||||||
|
"coords": [
|
||||||
|
0,
|
||||||
|
0,
|
||||||
|
0
|
||||||
|
],
|
||||||
|
"poses": []
|
||||||
|
},
|
||||||
|
"qr": {
|
||||||
|
"coords": [
|
||||||
|
0,
|
||||||
|
0,
|
||||||
|
0
|
||||||
|
],
|
||||||
|
"qr_value": "",
|
||||||
|
"model_id": ""
|
||||||
|
}
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"id": "m_1_1",
|
||||||
|
"row": 1,
|
||||||
|
"col": 1,
|
||||||
|
"front": {
|
||||||
|
"coords": [
|
||||||
|
0,
|
||||||
|
0,
|
||||||
|
0
|
||||||
|
],
|
||||||
|
"poses": []
|
||||||
|
},
|
||||||
|
"back": {
|
||||||
|
"coords": [
|
||||||
|
0,
|
||||||
|
0,
|
||||||
|
0
|
||||||
|
],
|
||||||
|
"poses": []
|
||||||
|
},
|
||||||
|
"qr": {
|
||||||
|
"coords": [
|
||||||
|
0,
|
||||||
|
0,
|
||||||
|
0
|
||||||
|
],
|
||||||
|
"qr_value": "",
|
||||||
|
"model_id": ""
|
||||||
|
}
|
||||||
}
|
}
|
||||||
]
|
]
|
||||||
@@ -1,5 +1,5 @@
|
|||||||
{
|
{
|
||||||
"map_dir": "/home/elephant/agv_pro_ros2/src/agv_pro_navigation2/map/",
|
"map_dir": "/home/elephant/agv_pro_ros2/install/agv_pro_navigation2/share/agv_pro_navigation2/map/",
|
||||||
"map_file": "map.yaml",
|
"map_file": "map.yaml",
|
||||||
"map_yaml": "/home/elephant/agv_pro_ros2/src/agv_pro_navigation2/map/map.yaml"
|
"map_yaml": "/home/elephant/agv_pro_ros2/install/agv_pro_navigation2/share/agv_pro_navigation2/map/map.yaml"
|
||||||
}
|
}
|
||||||
@@ -1,6 +1,6 @@
|
|||||||
{
|
{
|
||||||
"rows": 4,
|
"rows": 2,
|
||||||
"cols": 6,
|
"cols": 5,
|
||||||
"grid": [],
|
"grid": [],
|
||||||
"positions": [
|
"positions": [
|
||||||
{
|
{
|
||||||
@@ -8,15 +8,169 @@
|
|||||||
"col": 0,
|
"col": 0,
|
||||||
"side": "front",
|
"side": "front",
|
||||||
"coords": [
|
"coords": [
|
||||||
0.616485726055098,
|
0.7790541397954953,
|
||||||
-0.002587517923224651,
|
-1.4475345726592936,
|
||||||
-0.003483980050000001
|
0.04652525663733687
|
||||||
],
|
],
|
||||||
"poses": []
|
"poses": []
|
||||||
},
|
},
|
||||||
{
|
{
|
||||||
"row": 3,
|
"row": 0,
|
||||||
"col": 1,
|
"col": 1,
|
||||||
|
"side": "front",
|
||||||
|
"coords": [
|
||||||
|
1.7721798197110892,
|
||||||
|
-1.4172647050697822,
|
||||||
|
0.026346988400783075
|
||||||
|
],
|
||||||
|
"poses": []
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"row": 1,
|
||||||
|
"col": 1,
|
||||||
|
"side": "back",
|
||||||
|
"coords": [
|
||||||
|
2.0156283932242465,
|
||||||
|
-3.6286459327051652,
|
||||||
|
-3.1344357375846337
|
||||||
|
],
|
||||||
|
"poses": []
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"row": 1,
|
||||||
|
"col": 0,
|
||||||
|
"side": "back",
|
||||||
|
"coords": [
|
||||||
|
0.6542779107672178,
|
||||||
|
-3.610037554246379,
|
||||||
|
-3.139991515496645
|
||||||
|
],
|
||||||
|
"poses": []
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"row": 0,
|
||||||
|
"col": 2,
|
||||||
|
"side": "front",
|
||||||
|
"coords": [
|
||||||
|
3.032329928935363,
|
||||||
|
-1.4045725439516354,
|
||||||
|
0.019396580473191316
|
||||||
|
],
|
||||||
|
"poses": []
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"row": 0,
|
||||||
|
"col": 3,
|
||||||
|
"side": "front",
|
||||||
|
"coords": [
|
||||||
|
4.355614139761155,
|
||||||
|
-1.4037408856787776,
|
||||||
|
0.028879849807957422
|
||||||
|
],
|
||||||
|
"poses": []
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"row": 0,
|
||||||
|
"col": 4,
|
||||||
|
"side": "front",
|
||||||
|
"coords": [
|
||||||
|
5.591022741827066,
|
||||||
|
-1.4041991720435099,
|
||||||
|
0.007950673643717402
|
||||||
|
],
|
||||||
|
"poses": []
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"row": 1,
|
||||||
|
"col": 4,
|
||||||
|
"side": "back",
|
||||||
|
"coords": [
|
||||||
|
5.855580527635482,
|
||||||
|
-3.653374052468203,
|
||||||
|
-3.1205683154447357
|
||||||
|
],
|
||||||
|
"poses": []
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"row": 1,
|
||||||
|
"col": 4,
|
||||||
|
"side": "front",
|
||||||
|
"coords": [
|
||||||
|
5.855580527635482,
|
||||||
|
-3.653374052468203,
|
||||||
|
-3.1205683154447357
|
||||||
|
],
|
||||||
|
"poses": []
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"row": 1,
|
||||||
|
"col": 3,
|
||||||
|
"side": "back",
|
||||||
|
"coords": [
|
||||||
|
4.639109284799733,
|
||||||
|
-3.6545643029563504,
|
||||||
|
-3.103938599592417
|
||||||
|
],
|
||||||
|
"poses": []
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"row": 1,
|
||||||
|
"col": 3,
|
||||||
|
"side": "front",
|
||||||
|
"coords": [
|
||||||
|
4.639109284799733,
|
||||||
|
-3.6545643029563504,
|
||||||
|
-3.103938599592417
|
||||||
|
],
|
||||||
|
"poses": []
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"row": 1,
|
||||||
|
"col": 2,
|
||||||
|
"side": "back",
|
||||||
|
"coords": [
|
||||||
|
3.352049221669485,
|
||||||
|
-3.6285597877078626,
|
||||||
|
3.1386114980262145
|
||||||
|
],
|
||||||
|
"poses": []
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"row": 1,
|
||||||
|
"col": 2,
|
||||||
|
"side": "front",
|
||||||
|
"coords": [
|
||||||
|
3.352049221669485,
|
||||||
|
-3.6285597877078626,
|
||||||
|
3.1386114980262145
|
||||||
|
],
|
||||||
|
"poses": []
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"row": 1,
|
||||||
|
"col": 1,
|
||||||
|
"side": "front",
|
||||||
|
"coords": [
|
||||||
|
2.0156283932242465,
|
||||||
|
-3.6286459327051652,
|
||||||
|
-3.1344357375846337
|
||||||
|
],
|
||||||
|
"poses": []
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"row": 1,
|
||||||
|
"col": 0,
|
||||||
|
"side": "front",
|
||||||
|
"coords": [
|
||||||
|
0.6542779107672178,
|
||||||
|
-3.610037554246379,
|
||||||
|
-3.139991515496645
|
||||||
|
],
|
||||||
|
"poses": []
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"row": 2,
|
||||||
|
"col": 0,
|
||||||
"side": "back",
|
"side": "back",
|
||||||
"coords": [
|
"coords": [
|
||||||
0,
|
0,
|
||||||
@@ -25,98 +179,10 @@
|
|||||||
],
|
],
|
||||||
"poses": []
|
"poses": []
|
||||||
},
|
},
|
||||||
{
|
|
||||||
"row": 1,
|
|
||||||
"col": 1,
|
|
||||||
"side": "front",
|
|
||||||
"coords": [
|
|
||||||
-0.27906358987415997,
|
|
||||||
0.00411087876725537,
|
|
||||||
-0.00749475593
|
|
||||||
],
|
|
||||||
"poses": []
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"row": 0,
|
|
||||||
"col": 1,
|
|
||||||
"side": "front",
|
|
||||||
"coords": [
|
|
||||||
0.616485726055098,
|
|
||||||
-0.002587517923224651,
|
|
||||||
-0.003483980050000001
|
|
||||||
],
|
|
||||||
"poses": []
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"row": 0,
|
|
||||||
"col": 2,
|
|
||||||
"side": "front",
|
|
||||||
"coords": [
|
|
||||||
-0.27906358987415997,
|
|
||||||
0.00411087876725537,
|
|
||||||
-0.00749475593
|
|
||||||
],
|
|
||||||
"poses": []
|
|
||||||
},
|
|
||||||
{
|
{
|
||||||
"row": 2,
|
"row": 2,
|
||||||
"col": 1,
|
"col": 1,
|
||||||
"side": "shoot",
|
"side": "back",
|
||||||
"coords": [
|
|
||||||
-1.898244121263206,
|
|
||||||
-0.014324627152337432,
|
|
||||||
0.004533442980000002
|
|
||||||
],
|
|
||||||
"poses": []
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"row": 2,
|
|
||||||
"col": 0,
|
|
||||||
"side": "shoot",
|
|
||||||
"coords": [
|
|
||||||
-0.9528404539697249,
|
|
||||||
-0.01004755255507813,
|
|
||||||
0.005515614170000002
|
|
||||||
],
|
|
||||||
"poses": []
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"row": 0,
|
|
||||||
"col": 1,
|
|
||||||
"side": "shoot",
|
|
||||||
"coords": [
|
|
||||||
-0.9528404539697249,
|
|
||||||
-0.01004755255507813,
|
|
||||||
0.005515614170000002
|
|
||||||
],
|
|
||||||
"poses": []
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"row": 1,
|
|
||||||
"col": 1,
|
|
||||||
"side": "shoot",
|
|
||||||
"coords": [
|
|
||||||
-0.9528404539697249,
|
|
||||||
-0.01004755255507813,
|
|
||||||
0.005515614170000002
|
|
||||||
],
|
|
||||||
"poses": []
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"row": 0,
|
|
||||||
"col": 0,
|
|
||||||
"side": "shoot",
|
|
||||||
"coords": [
|
|
||||||
0,
|
|
||||||
0,
|
|
||||||
0
|
|
||||||
],
|
|
||||||
"poses": []
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"row": 1,
|
|
||||||
"col": 0,
|
|
||||||
"side": "shoot",
|
|
||||||
"coords": [
|
"coords": [
|
||||||
0,
|
0,
|
||||||
0,
|
0,
|
||||||
@@ -124,5 +190,13 @@
|
|||||||
],
|
],
|
||||||
"poses": []
|
"poses": []
|
||||||
}
|
}
|
||||||
|
],
|
||||||
|
"arm_initial_pose": [
|
||||||
|
-90.333323,
|
||||||
|
-90.079952,
|
||||||
|
0.160037,
|
||||||
|
-90.571318,
|
||||||
|
0.093654,
|
||||||
|
22.232898
|
||||||
]
|
]
|
||||||
}
|
}
|
||||||
@@ -9,12 +9,12 @@
|
|||||||
"name": "front_1",
|
"name": "front_1",
|
||||||
"photo_type": "front",
|
"photo_type": "front",
|
||||||
"arm_angles": [
|
"arm_angles": [
|
||||||
0,
|
46.333359,
|
||||||
0,
|
-90.759907,
|
||||||
0,
|
7.5e-05,
|
||||||
0,
|
-89.156729,
|
||||||
0,
|
-15.823339,
|
||||||
0
|
20.403326
|
||||||
],
|
],
|
||||||
"speed": 500,
|
"speed": 500,
|
||||||
"description": ""
|
"description": ""
|
||||||
@@ -24,12 +24,12 @@
|
|||||||
"name": "front_2",
|
"name": "front_2",
|
||||||
"photo_type": "front",
|
"photo_type": "front",
|
||||||
"arm_angles": [
|
"arm_angles": [
|
||||||
0,
|
46.333325,
|
||||||
0,
|
-88.800201,
|
||||||
0,
|
0.000164,
|
||||||
0,
|
-89.1564,
|
||||||
0,
|
-15.823202,
|
||||||
0
|
20.403374
|
||||||
],
|
],
|
||||||
"speed": 500,
|
"speed": 500,
|
||||||
"description": ""
|
"description": ""
|
||||||
@@ -67,5 +67,13 @@
|
|||||||
],
|
],
|
||||||
"description": "",
|
"description": "",
|
||||||
"notes": ""
|
"notes": ""
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"id": "m_1779286436",
|
||||||
|
"name": "m2",
|
||||||
|
"serial_prefix": "",
|
||||||
|
"poses": [],
|
||||||
|
"description": "",
|
||||||
|
"notes": ""
|
||||||
}
|
}
|
||||||
]
|
]
|
||||||
+279
-106
@@ -23,6 +23,8 @@ import requests
|
|||||||
import cv2
|
import cv2
|
||||||
import numpy as np
|
import numpy as np
|
||||||
|
|
||||||
|
from utils.nav2_navigator import Nav2Navigator, Nav2Status
|
||||||
|
|
||||||
logger = logging.getLogger(__name__)
|
logger = logging.getLogger(__name__)
|
||||||
|
|
||||||
ROS2_SETUP_CMD = "source /opt/ros/humble/setup.bash && source ~/agv_pro_ros2/install/setup.bash"
|
ROS2_SETUP_CMD = "source /opt/ros/humble/setup.bash && source ~/agv_pro_ros2/install/setup.bash"
|
||||||
@@ -42,6 +44,8 @@ class MissionStatus(str, Enum):
|
|||||||
PAUSED = "paused"
|
PAUSED = "paused"
|
||||||
COMPLETED = "completed"
|
COMPLETED = "completed"
|
||||||
WAITING_QR = "waiting_qr"
|
WAITING_QR = "waiting_qr"
|
||||||
|
WAITING_ERROR = "waiting_error"
|
||||||
|
WAITING_STEP = "waiting_step"
|
||||||
|
|
||||||
|
|
||||||
class MissionExecutorV3:
|
class MissionExecutorV3:
|
||||||
@@ -71,6 +75,22 @@ class MissionExecutorV3:
|
|||||||
self._qr_event = threading.Event()
|
self._qr_event = threading.Event()
|
||||||
self._qr_value: Optional[str] = None
|
self._qr_value: Optional[str] = None
|
||||||
|
|
||||||
|
# 错误弹窗
|
||||||
|
self._error_choice = None # "skip" or "abort"
|
||||||
|
|
||||||
|
# 单步执行
|
||||||
|
self._single_step_mode = False
|
||||||
|
self._step_choice = None # "confirm", "retry", "abort"
|
||||||
|
self._error_mode = False # True when waiting for error resolution
|
||||||
|
|
||||||
|
# 错误弹窗
|
||||||
|
self._error_choice = None # "skip" or "abort"
|
||||||
|
|
||||||
|
# 单步执行
|
||||||
|
self._single_step_mode = False
|
||||||
|
self._step_choice = None # "confirm", "retry", "abort"
|
||||||
|
self._error_mode = False # True when waiting for error resolution
|
||||||
|
|
||||||
# 设备
|
# 设备
|
||||||
from .arm_client import ArmClient
|
from .arm_client import ArmClient
|
||||||
self.arm_client = ArmClient(
|
self.arm_client = ArmClient(
|
||||||
@@ -84,6 +104,9 @@ class MissionExecutorV3:
|
|||||||
baudrate=config.get("baudrate", 1000000)
|
baudrate=config.get("baudrate", 1000000)
|
||||||
)
|
)
|
||||||
|
|
||||||
|
# Nav2 导航器（直接使用 rclpy BasicNavigator API，比 subprocess 更可靠）
|
||||||
|
self._nav = Nav2Navigator()
|
||||||
|
|
||||||
# ==================== 连接 ====================
|
# ==================== 连接 ====================
|
||||||
|
|
||||||
def connect_all(self) -> Dict[str, bool]:
|
def connect_all(self) -> Dict[str, bool]:
|
||||||
@@ -105,10 +128,23 @@ class MissionExecutorV3:
|
|||||||
machines: list,
|
machines: list,
|
||||||
qr_configs: list,
|
qr_configs: list,
|
||||||
models: list,
|
models: list,
|
||||||
|
single_step: bool = False,
|
||||||
|
options: dict = None,
|
||||||
) -> dict:
|
) -> dict:
|
||||||
"""
|
"""
|
||||||
执行完整拍摄任务。
|
执行完整拍摄任务。
|
||||||
|
|
||||||
|
Args:
|
||||||
|
options: 任务步骤控制开关
|
||||||
|
- arm_init: 是否执行机械臂位置初始化
|
||||||
|
- agv_move: 是否执行AGV移动
|
||||||
|
- qr_scan: 是否执行二维码识别
|
||||||
|
- front_photo: 是否执行正面拍照
|
||||||
|
- back_photo: 是否执行背面拍照
|
||||||
|
"""
|
||||||
|
"""
|
||||||
|
执行完整拍摄任务。
|
||||||
|
|
||||||
Args:
|
Args:
|
||||||
mission_config: {rows, cols, grid, positions}
|
mission_config: {rows, cols, grid, positions}
|
||||||
machines: [{id, row, col, front: {coords}, back: {coords}}]
|
machines: [{id, row, col, front: {coords}, back: {coords}}]
|
||||||
@@ -131,8 +167,23 @@ class MissionExecutorV3:
|
|||||||
grid = mission_config.get("grid", [])
|
grid = mission_config.get("grid", [])
|
||||||
positions = mission_config.get("positions", [])
|
positions = mission_config.get("positions", [])
|
||||||
|
|
||||||
|
# 如果 grid 为空，从 machines 重建
|
||||||
|
if not grid or all(not any(rw) if isinstance(rw, list) else True for rw in grid):
|
||||||
|
try:
|
||||||
|
cfg_dir = os.path.join(os.path.dirname(os.path.abspath(__file__)), "..", "data")
|
||||||
|
with open(os.path.join(cfg_dir, "machines_config.json")) as jf:
|
||||||
|
machines = json.load(jf)
|
||||||
|
grid = [[False] * cols for _ in range(rows)]
|
||||||
|
for m in machines:
|
||||||
|
r = int(m.get("row", 0))
|
||||||
|
c = int(m.get("col", 0))
|
||||||
|
if 0 <= r < rows and 0 <= c < cols:
|
||||||
|
grid[r][c] = True
|
||||||
|
except Exception:
|
||||||
|
grid = [[False] * cols for _ in range(rows)]
|
||||||
|
|
||||||
# 1. 生成蛇形路径
|
# 1. 生成蛇形路径
|
||||||
path = self._build_snake_path(rows, cols, grid)
|
path = MissionExecutorV3._build_snake_path(rows, cols, grid)
|
||||||
if not path:
|
if not path:
|
||||||
self._log("❌ 网格中没有机器，任务终止")
|
self._log("❌ 网格中没有机器，任务终止")
|
||||||
self.report["error"] = "No machines in grid"
|
self.report["error"] = "No machines in grid"
|
||||||
@@ -153,25 +204,43 @@ class MissionExecutorV3:
|
|||||||
"photos_back": 0,
|
"photos_back": 0,
|
||||||
} for (r, c) in path]
|
} for (r, c) in path]
|
||||||
|
|
||||||
# 初始化任务列表
|
# 任务步骤控制开关
|
||||||
self.report["tasks"] = [{
|
if options is None:
|
||||||
"row": r, "col": c,
|
options = {}
|
||||||
"machine_id": f"m_{r}_{c}",
|
opt_arm_init = options.get("arm_init", True)
|
||||||
"label": f"{r+1}-{c+1}",
|
opt_agv_move = options.get("agv_move", True)
|
||||||
"status": "pending",
|
# 机械臂初始化并入 AGV 移动：只要开移动，就先初始化机械臂
|
||||||
"step": "等待",
|
if opt_agv_move:
|
||||||
"qr_value": None,
|
opt_arm_init = True
|
||||||
"photos_front": 0,
|
opt_qr_scan = options.get("qr_scan", True)
|
||||||
"photos_back": 0,
|
opt_front_photo = options.get("front_photo", True)
|
||||||
} for (r, c) in path]
|
opt_back_photo = options.get("back_photo", True)
|
||||||
|
self._log(f"⚙️ 任务步骤: agv_move={opt_agv_move}, "
|
||||||
|
f"qr_scan={opt_qr_scan}, front={opt_front_photo}, back={opt_back_photo}")
|
||||||
|
|
||||||
|
# 机械臂初始姿态（AGV 移动前恢复）
|
||||||
|
arm_initial_pose = mission_config.get("arm_initial_pose", [0.0] * 6)
|
||||||
|
has_arm_pose = self.arm_client and any(abs(a) > 0.01 for a in arm_initial_pose)
|
||||||
|
|
||||||
# 2. 逐台执行
|
# 2. 逐台执行
|
||||||
for idx, (r, c) in enumerate(path):
|
machine_idx = 0
|
||||||
|
while machine_idx < len(path):
|
||||||
if self._stop.is_set():
|
if self._stop.is_set():
|
||||||
self._log("⏹️ 任务已停止")
|
self._log("⏹️ 任务已停止")
|
||||||
break
|
break
|
||||||
|
|
||||||
self._wait_pause()
|
self._wait_pause()
|
||||||
|
r, c = path[machine_idx]
|
||||||
|
rl, cl = r + 1, c + 1
|
||||||
|
|
||||||
|
# 恢复机械臂初始姿态
|
||||||
|
if opt_arm_init and has_arm_pose:
|
||||||
|
self._log(" 🦾 恢复机械臂初始姿态")
|
||||||
|
try:
|
||||||
|
self.arm_client.set_angles(arm_initial_pose, speed=500)
|
||||||
|
time.sleep(2)
|
||||||
|
except Exception as e:
|
||||||
|
self._log(f" ⚠️ 机械臂初始化失败: {e}")
|
||||||
|
|
||||||
# 更新任务状态 → 正面开始
|
# 更新任务状态 → 正面开始
|
||||||
task = self._get_task(r, c)
|
task = self._get_task(r, c)
|
||||||
@@ -182,27 +251,36 @@ class MissionExecutorV3:
|
|||||||
machine_id = f"m_{r}_{c}"
|
machine_id = f"m_{r}_{c}"
|
||||||
machine = next((m for m in machines if m.get("id") == machine_id), None)
|
machine = next((m for m in machines if m.get("id") == machine_id), None)
|
||||||
if not machine:
|
if not machine:
|
||||||
self._log(f"⚠️ 机器 {r+1}-{c+1} 不存在，跳过")
|
self._log(f"⚠️ 机器 {rl}-{cl} 不存在，跳过")
|
||||||
|
machine_idx += 1
|
||||||
continue
|
continue
|
||||||
|
|
||||||
rl, cl = r + 1, c + 1 # 显示用的 1-based
|
|
||||||
|
|
||||||
# --- 正面 ---
|
# --- 正面 ---
|
||||||
self._log(f"📍 机器 {rl}-{cl} 进入正面点位")
|
self._log(f"📍 机器 {rl}-{cl} 进入正面点位")
|
||||||
self._step(f"机器 {rl}-{cl} 正面")
|
self._step(f"机器 {rl}-{cl} 正面")
|
||||||
|
|
||||||
# 导航到正面点位
|
# 导航到正面点位
|
||||||
front_pt = self._find_point(positions, r, c, "front")
|
if opt_agv_move:
|
||||||
if front_pt and self._has_coords(front_pt):
|
front_pt = self._find_point(positions, r, c, "front")
|
||||||
if not self._navigate(front_pt, "正面"):
|
if front_pt and self._has_coords(front_pt):
|
||||||
self._log(f"⚠️ 导航失败，尝试继续")
|
if not self._navigate(front_pt, "正面"):
|
||||||
|
self._log(f"⚠️ 导航失败，尝试继续")
|
||||||
|
choice = self._wait_error(f"机器 {rl}-{cl} 正面导航失败")
|
||||||
|
if choice == "abort":
|
||||||
|
break
|
||||||
|
else:
|
||||||
|
self._log(f"⚠️ 无正面点位坐标")
|
||||||
else:
|
else:
|
||||||
self._log(f"⚠️ 无正面点位坐标")
|
self._log(" ⏭️ 跳过AGV移动（正面）")
|
||||||
|
|
||||||
# 扫描二维码
|
# 扫描二维码
|
||||||
qr_value = self._scan_qr_with_poses(qr_configs)
|
qr_value = None
|
||||||
if self._stop.is_set():
|
if opt_qr_scan:
|
||||||
break
|
qr_value = self._scan_qr_with_poses(qr_configs)
|
||||||
|
if self._stop.is_set():
|
||||||
|
break
|
||||||
|
else:
|
||||||
|
self._log(" ⏭️ 跳过二维码识别")
|
||||||
|
|
||||||
# 查机型 + 更新任务步骤
|
# 查机型 + 更新任务步骤
|
||||||
model_name = self._lookup_model(qr_value)
|
model_name = self._lookup_model(qr_value)
|
||||||
@@ -213,13 +291,16 @@ class MissionExecutorV3:
|
|||||||
task["step"] = "正面拍照"
|
task["step"] = "正面拍照"
|
||||||
|
|
||||||
# 正面拍照
|
# 正面拍照
|
||||||
model = self._find_model(models, model_name)
|
if opt_front_photo:
|
||||||
if model:
|
model = self._find_model(models, model_name)
|
||||||
self._shoot(model, "front", rl, cl, qr_value or "unknown")
|
if model:
|
||||||
|
self._shoot(model, "front", rl, cl, qr_value or "unknown")
|
||||||
|
else:
|
||||||
|
self._log(f" ⚠️ 未找到机型 {model_name}")
|
||||||
else:
|
else:
|
||||||
self._log(f" ⚠️ 未找到机型 {model_name}")
|
self._log(" ⏭️ 跳过正面拍照")
|
||||||
|
|
||||||
self._progress(idx, 1)
|
self._progress(machine_idx, 1)
|
||||||
|
|
||||||
# --- 背面 ---
|
# --- 背面 ---
|
||||||
if task:
|
if task:
|
||||||
@@ -228,28 +309,53 @@ class MissionExecutorV3:
|
|||||||
self._step(f"机器 {rl}-{cl} 背面")
|
self._step(f"机器 {rl}-{cl} 背面")
|
||||||
|
|
||||||
# 导航到背面点位
|
# 导航到背面点位
|
||||||
back_pt = self._find_point(positions, r + 1, c, "back")
|
if opt_agv_move:
|
||||||
if back_pt and self._has_coords(back_pt):
|
back_pt = self._find_point(positions, r + 1, c, "back")
|
||||||
if not self._navigate(back_pt, "背面"):
|
if back_pt and self._has_coords(back_pt):
|
||||||
self._log(f"⚠️ 导航失败，尝试继续")
|
if not self._navigate(back_pt, "背面"):
|
||||||
|
self._log(f"⚠️ 导航失败，尝试继续")
|
||||||
|
choice = self._wait_error(f"机器 {rl}-{cl} 背面导航失败")
|
||||||
|
if choice == "abort":
|
||||||
|
break
|
||||||
|
else:
|
||||||
|
self._log(f"⚠️ 无背面点位坐标")
|
||||||
else:
|
else:
|
||||||
self._log(f"⚠️ 无背面点位坐标")
|
self._log(" ⏭️ 跳过AGV移动（背面）")
|
||||||
|
|
||||||
# 背面拍照
|
# 背面拍照
|
||||||
if model:
|
if opt_back_photo:
|
||||||
self._shoot(model, "back", rl, cl, qr_value or "unknown")
|
if model:
|
||||||
|
self._shoot(model, "back", rl, cl, qr_value or "unknown")
|
||||||
|
else:
|
||||||
|
self._log(" ⏭️ 跳过背面拍照")
|
||||||
|
|
||||||
# 标记任务完成
|
# 标记任务完成
|
||||||
if task:
|
if task:
|
||||||
task["status"] = "completed"
|
task["status"] = "completed"
|
||||||
task["step"] = "完成"
|
task["step"] = "完成"
|
||||||
self._progress(idx, 2)
|
self._progress(machine_idx, 2)
|
||||||
|
|
||||||
|
# 单步执行：等待用户确认
|
||||||
|
if single_step and not self._stop.is_set():
|
||||||
|
choice = self._wait_step_confirm(rl, cl)
|
||||||
|
if choice == "abort":
|
||||||
|
break
|
||||||
|
elif choice == "retry":
|
||||||
|
if task:
|
||||||
|
task["status"] = "pending"
|
||||||
|
task["step"] = "重试开始"
|
||||||
|
self._progress(machine_idx, 0)
|
||||||
|
continue # 不递增 machine_idx，重新执行
|
||||||
|
|
||||||
|
machine_idx += 1
|
||||||
|
|
||||||
# 3. 回到出发点
|
# 3. 回到出发点
|
||||||
if not self._stop.is_set():
|
if not self._stop.is_set() and opt_agv_move:
|
||||||
self._step("返回出发点")
|
self._step("返回出发点")
|
||||||
self._log("→ 返回 (0, 0)")
|
self._log("→ 返回 (0, 0)")
|
||||||
self._nav2_go_to_point(0, 0, 0, timeout_sec=60)
|
self._nav2_go_to_point(0, 0, 0, timeout_sec=60)
|
||||||
|
elif not self._stop.is_set():
|
||||||
|
self._log("⏭️ 跳过返回出发点")
|
||||||
|
|
||||||
elapsed = time.time() - start_time
|
elapsed = time.time() - start_time
|
||||||
return self._finish(elapsed)
|
return self._finish(elapsed)
|
||||||
@@ -275,17 +381,18 @@ class MissionExecutorV3:
|
|||||||
|
|
||||||
# ==================== 蛇形路径 ====================
|
# ==================== 蛇形路径 ====================
|
||||||
|
|
||||||
def _build_snake_path(self, rows: int, cols: int, grid: list) -> list:
|
@staticmethod
|
||||||
|
def _build_snake_path(rows: int, cols: int, grid: list) -> list:
|
||||||
"""奇数行(0,2,4...)左→右，偶数行(1,3,5...)右→左"""
|
"""奇数行(0,2,4...)左→右，偶数行(1,3,5...)右→左"""
|
||||||
path = []
|
path = []
|
||||||
for r in range(rows):
|
for r in range(rows):
|
||||||
if r % 2 == 0:
|
if r % 2 == 0:
|
||||||
for c in range(cols):
|
for c in range(cols):
|
||||||
if self._has_machine(grid, r, c):
|
if MissionExecutorV3._has_machine(grid, r, c):
|
||||||
path.append((r, c))
|
path.append((r, c))
|
||||||
else:
|
else:
|
||||||
for c in range(cols - 1, -1, -1):
|
for c in range(cols - 1, -1, -1):
|
||||||
if self._has_machine(grid, r, c):
|
if MissionExecutorV3._has_machine(grid, r, c):
|
||||||
path.append((r, c))
|
path.append((r, c))
|
||||||
return path
|
return path
|
||||||
|
|
||||||
@@ -298,6 +405,52 @@ class MissionExecutorV3:
|
|||||||
return c < len(row) and bool(row[c])
|
return c < len(row) and bool(row[c])
|
||||||
return False
|
return False
|
||||||
|
|
||||||
|
@staticmethod
|
||||||
|
def _build_grid_from_machines(rows: int, cols: int, machines: list) -> list:
|
||||||
|
"""从机器列表重建 grid 矩阵"""
|
||||||
|
if not machines:
|
||||||
|
return [[False] * cols for _ in range(rows)]
|
||||||
|
max_r = max(int(m.get("row", 0)) for m in machines) + 1
|
||||||
|
max_c = max(int(m.get("col", 0)) for m in machines) + 1
|
||||||
|
gr = max(rows, max_r)
|
||||||
|
gc = max(cols, max_c)
|
||||||
|
grid = [[False] * gc for _ in range(gr)]
|
||||||
|
for m in machines:
|
||||||
|
r = int(m.get("row", 0))
|
||||||
|
c = int(m.get("col", 0))
|
||||||
|
if 0 <= r < gr and 0 <= c < gc:
|
||||||
|
grid[r][c] = True
|
||||||
|
return grid
|
||||||
|
|
||||||
|
@staticmethod
|
||||||
|
def pre_generate_tasks(mission_config: dict) -> list:
|
||||||
|
"""从网格配置预生成任务列表（用于 UI 展示，无需启动执行器）"""
|
||||||
|
rows = int(mission_config.get("rows", 1))
|
||||||
|
cols = int(mission_config.get("cols", 1))
|
||||||
|
grid = mission_config.get("grid", [])
|
||||||
|
|
||||||
|
# 如果 grid 为空但从 machines 重建
|
||||||
|
if not grid and machines:
|
||||||
|
grid = MissionExecutorV3._build_grid_from_machines(rows, cols, machines)
|
||||||
|
if grid:
|
||||||
|
rows = len(grid)
|
||||||
|
cols = len(grid[0]) if grid else cols
|
||||||
|
|
||||||
|
path = MissionExecutorV3._build_snake_path(rows, cols, grid)
|
||||||
|
tasks = []
|
||||||
|
for (r, c) in path:
|
||||||
|
tasks.append({
|
||||||
|
"row": r, "col": c,
|
||||||
|
"machine_id": f"m_{r}_{c}",
|
||||||
|
"label": f"{r+1}-{c+1}",
|
||||||
|
"status": "pending",
|
||||||
|
"step": "等待",
|
||||||
|
"qr_value": None,
|
||||||
|
"photos_front": 0,
|
||||||
|
"photos_back": 0,
|
||||||
|
})
|
||||||
|
return tasks
|
||||||
|
|
||||||
# ==================== 点位查找 ====================
|
# ==================== 点位查找 ====================
|
||||||
|
|
||||||
@staticmethod
|
@staticmethod
|
||||||
@@ -512,6 +665,7 @@ class MissionExecutorV3:
|
|||||||
"step": self.report["step"],
|
"step": self.report["step"],
|
||||||
"progress": self.report["progress"],
|
"progress": self.report["progress"],
|
||||||
"total": self.report["total"],
|
"total": self.report["total"],
|
||||||
|
"tasks": self.report.get("tasks", []),
|
||||||
}
|
}
|
||||||
|
|
||||||
def get_logs(self) -> dict:
|
def get_logs(self) -> dict:
|
||||||
@@ -545,6 +699,82 @@ class MissionExecutorV3:
|
|||||||
99
|
99
|
||||||
)
|
)
|
||||||
|
|
||||||
|
# ==================== 错误弹窗 ====================
|
||||||
|
|
||||||
|
def _wait_error(self, msg: str) -> str:
|
||||||
|
"""阻塞等待用户选择：skip（跳过）或 abort（中断）"""
|
||||||
|
self.status = MissionStatus.WAITING_ERROR
|
||||||
|
self.report["status"] = "waiting_error"
|
||||||
|
self.report["step"] = msg
|
||||||
|
self.report["error"] = msg
|
||||||
|
self._log(f"⚠️ 错误处理: {msg}")
|
||||||
|
|
||||||
|
self._error_choice = None
|
||||||
|
self._error_mode = True
|
||||||
|
start = time.time()
|
||||||
|
while self._error_choice is None and not self._stop.is_set():
|
||||||
|
time.sleep(0.2)
|
||||||
|
if time.time() - start > 600: # 10分钟超时 → 跳过
|
||||||
|
self._error_choice = "skip"
|
||||||
|
|
||||||
|
choice = self._error_choice or "skip"
|
||||||
|
self._error_choice = None
|
||||||
|
self._error_mode = False
|
||||||
|
|
||||||
|
if choice == "abort":
|
||||||
|
self._stop.set()
|
||||||
|
self._log("⏹️ 用户选择中断")
|
||||||
|
else:
|
||||||
|
self._log("⏭️ 用户选择跳过")
|
||||||
|
|
||||||
|
# 恢复状态
|
||||||
|
self.status = MissionStatus.RUNNING if not self._single_step_mode else MissionStatus.WAITING_STEP
|
||||||
|
self.report["status"] = self.status.value
|
||||||
|
self.report["error"] = None
|
||||||
|
return choice
|
||||||
|
|
||||||
|
def set_error_choice(self, choice: str):
|
||||||
|
"""外部 API 设置错误处理选择"""
|
||||||
|
self._error_choice = choice
|
||||||
|
|
||||||
|
# ==================== 单步执行 ====================
|
||||||
|
|
||||||
|
def _wait_step_confirm(self, row_label: int, col_label: int) -> str:
|
||||||
|
"""单步执行：等待用户确认/重试/中断"""
|
||||||
|
self.status = MissionStatus.WAITING_STEP
|
||||||
|
self.report["status"] = "waiting_step"
|
||||||
|
self.report["step"] = f"机器 {row_label}-{col_label} 完成，等待确认"
|
||||||
|
self.report["current_step"] = {
|
||||||
|
"row": row_label - 1, "col": col_label - 1,
|
||||||
|
"label": f"{row_label}-{col_label}"
|
||||||
|
}
|
||||||
|
self._log(f"⏸️ 单步执行: 机器 {row_label}-{col_label} 完成，等待确认...")
|
||||||
|
|
||||||
|
self._step_choice = None
|
||||||
|
start = time.time()
|
||||||
|
while self._step_choice is None and not self._stop.is_set():
|
||||||
|
time.sleep(0.2)
|
||||||
|
if time.time() - start > 600: # 10分钟超时 → 确认
|
||||||
|
self._step_choice = "confirm"
|
||||||
|
|
||||||
|
choice = self._step_choice or "confirm"
|
||||||
|
self._step_choice = None
|
||||||
|
self.report.pop("current_step", None)
|
||||||
|
|
||||||
|
if choice == "abort":
|
||||||
|
self._stop.set()
|
||||||
|
self._log("⏹️ 用户选择中断")
|
||||||
|
elif choice == "retry":
|
||||||
|
self._log(f"🔄 用户选择重试机器 {row_label}-{col_label}")
|
||||||
|
else:
|
||||||
|
self._log(f"✅ 用户确认机器 {row_label}-{col_label}")
|
||||||
|
|
||||||
|
return choice
|
||||||
|
|
||||||
|
def set_step_choice(self, choice: str):
|
||||||
|
"""外部 API 设置单步执行选择"""
|
||||||
|
self._step_choice = choice
|
||||||
|
|
||||||
# ==================== Nav2 导航 ====================
|
# ==================== Nav2 导航 ====================
|
||||||
# (保留原实现)
|
# (保留原实现)
|
||||||
|
|
||||||
@@ -559,72 +789,15 @@ class MissionExecutorV3:
|
|||||||
|
|
||||||
def _nav2_go_to_point(self, x: float, y: float, yaw: float = 0.0,
|
def _nav2_go_to_point(self, x: float, y: float, yaw: float = 0.0,
|
||||||
timeout_sec: float = 120.0) -> bool:
|
timeout_sec: float = 120.0) -> bool:
|
||||||
if not self._nav2_check_available():
|
"""使用 Nav2Navigator 直接发送导航目标（blocking 模式，等待完成）"""
|
||||||
logger.error("Nav2 action server 不可用")
|
|
||||||
return False
|
|
||||||
|
|
||||||
qz = math.sin(yaw / 2.0)
|
|
||||||
qw = math.cos(yaw / 2.0)
|
|
||||||
|
|
||||||
pose_yaml = (
|
|
||||||
f"pose:\n"
|
|
||||||
f" header:\n"
|
|
||||||
f" stamp:\n"
|
|
||||||
f" sec: 0\n"
|
|
||||||
f" nanosec: 0\n"
|
|
||||||
f" frame_id: 'map'\n"
|
|
||||||
f" pose:\n"
|
|
||||||
f" position:\n"
|
|
||||||
f" x: {x}\n"
|
|
||||||
f" y: {y}\n"
|
|
||||||
f" z: 0.0\n"
|
|
||||||
f" orientation:\n"
|
|
||||||
f" x: 0.0\n"
|
|
||||||
f" y: 0.0\n"
|
|
||||||
f" z: {qz}\n"
|
|
||||||
f" w: {qw}"
|
|
||||||
)
|
|
||||||
|
|
||||||
cmd = (
|
|
||||||
f"ros2 action send_goal /navigate_to_pose "
|
|
||||||
f"nav2_msgs/action/NavigateToPose "
|
|
||||||
f"'{pose_yaml}' --feedback"
|
|
||||||
)
|
|
||||||
full_cmd = f"bash -c '{ROS2_SETUP_CMD} && {cmd}'"
|
|
||||||
|
|
||||||
try:
|
try:
|
||||||
process = subprocess.Popen(
|
logger.info(f"🧭 导航到目标: ({x:.3f}, {y:.3f}), yaw={math.degrees(yaw):.1f}°")
|
||||||
full_cmd, shell=True,
|
ok = self._nav.navigate_to_pose(x, y, yaw, timeout_sec=timeout_sec, blocking=True)
|
||||||
stdout=subprocess.PIPE, stderr=subprocess.PIPE, text=True
|
if ok:
|
||||||
)
|
logger.info(f"✅ 导航成功到达 ({x:.3f}, {y:.3f})")
|
||||||
succeeded = False
|
else:
|
||||||
elapsed = 0.0
|
logger.warning(f"⚠️ 导航失败或超时 ({x:.3f}, {y:.3f})")
|
||||||
while elapsed < timeout_sec:
|
return ok
|
||||||
import select
|
|
||||||
reads = [process.stdout]
|
|
||||||
ready, _, _ = select.select(reads, [], [], 1.0)
|
|
||||||
elapsed += 1.0
|
|
||||||
if process.stdout in ready:
|
|
||||||
line = process.stdout.readline()
|
|
||||||
if not line:
|
|
||||||
break
|
|
||||||
ls = line.strip()
|
|
||||||
if "succeeded" in ls.lower():
|
|
||||||
succeeded = True
|
|
||||||
break
|
|
||||||
elif "failed" in ls.lower() or "aborted" in ls.lower():
|
|
||||||
break
|
|
||||||
elif "canceled" in ls.lower() or "cancelled" in ls.lower():
|
|
||||||
break
|
|
||||||
if process.poll() is not None:
|
|
||||||
break
|
|
||||||
if process.poll() is None:
|
|
||||||
process.terminate()
|
|
||||||
try:
|
|
||||||
process.wait(timeout=3)
|
|
||||||
except subprocess.TimeoutExpired:
|
|
||||||
process.kill()
|
|
||||||
return succeeded
|
|
||||||
except Exception as e:
|
except Exception as e:
|
||||||
logger.error(f"Nav2 异常: {e}")
|
logger.error(f"Nav2 异常: {e}")
|
||||||
return False
|
return False
|
||||||
|
|||||||
+76
-4
@@ -4,7 +4,7 @@
|
|||||||
<meta charset="UTF-8">
|
<meta charset="UTF-8">
|
||||||
<meta name="viewport" content="width=device-width, initial-scale=1.0">
|
<meta name="viewport" content="width=device-width, initial-scale=1.0">
|
||||||
<title>运行监控 - AGV 拍摄系统</title>
|
<title>运行监控 - AGV 拍摄系统</title>
|
||||||
<link rel="stylesheet" href="/static/css/style.css">
|
<link rel="stylesheet" href="/static/css/style.css?v=20260527b">
|
||||||
</head>
|
</head>
|
||||||
<body>
|
<body>
|
||||||
<div id="app">
|
<div id="app">
|
||||||
@@ -48,6 +48,46 @@
|
|||||||
</div>
|
</div>
|
||||||
</section>
|
</section>
|
||||||
|
|
||||||
|
<!-- 任务步骤控制开关 -->
|
||||||
|
<section class="card">
|
||||||
|
<h2>🎛️ 任务步骤控制</h2>
|
||||||
|
<p class="hint" style="margin-bottom:12px">关闭的步骤将在本次任务中跳过</p>
|
||||||
|
<div class="toggle-grid">
|
||||||
|
<div class="toggle-item">
|
||||||
|
<label class="toggle-switch">
|
||||||
|
<input type="checkbox" v-model="agvMoveEnabled">
|
||||||
|
<span class="toggle-slider"></span>
|
||||||
|
</label>
|
||||||
|
<span class="toggle-label">🚗 AGV移动</span>
|
||||||
|
<span class="toggle-hint">含机械臂初始化，按之字形路线移动到各点位</span>
|
||||||
|
</div>
|
||||||
|
<div class="toggle-item">
|
||||||
|
<label class="toggle-switch">
|
||||||
|
<input type="checkbox" v-model="qrScanEnabled">
|
||||||
|
<span class="toggle-slider"></span>
|
||||||
|
</label>
|
||||||
|
<span class="toggle-label">📷 识别二维码</span>
|
||||||
|
<span class="toggle-hint">调整机械臂姿态扫描二维码</span>
|
||||||
|
</div>
|
||||||
|
<div class="toggle-item">
|
||||||
|
<label class="toggle-switch">
|
||||||
|
<input type="checkbox" v-model="frontPhotoEnabled">
|
||||||
|
<span class="toggle-slider"></span>
|
||||||
|
</label>
|
||||||
|
<span class="toggle-label">📸 拍正面照</span>
|
||||||
|
<span class="toggle-hint">按机型正面姿态拍照</span>
|
||||||
|
</div>
|
||||||
|
<div class="toggle-item">
|
||||||
|
<label class="toggle-switch">
|
||||||
|
<input type="checkbox" v-model="backPhotoEnabled">
|
||||||
|
<span class="toggle-slider"></span>
|
||||||
|
</label>
|
||||||
|
<span class="toggle-label">📸 拍背面照</span>
|
||||||
|
<span class="toggle-hint">按机型背面姿态拍照</span>
|
||||||
|
</div>
|
||||||
|
</div>
|
||||||
|
</section>
|
||||||
|
|
||||||
<!-- 任务清单 -->
|
<!-- 任务清单 -->
|
||||||
<section class="card" v-if="tasks.length > 0">
|
<section class="card" v-if="tasks.length > 0">
|
||||||
<h2>📋 任务清单 ([[ tasks.length ]] 台机器)</h2>
|
<h2>📋 任务清单 ([[ tasks.length ]] 台机器)</h2>
|
||||||
@@ -85,8 +125,15 @@
|
|||||||
<!-- 实时预览 -->
|
<!-- 实时预览 -->
|
||||||
<section class="card">
|
<section class="card">
|
||||||
<h2>📷 摄像头预览</h2>
|
<h2>📷 摄像头预览</h2>
|
||||||
<div class="camera-full">
|
<div class="camera-dual">
|
||||||
<img :src="previewUrl" @error="onPreviewError">
|
<div class="camera-box">
|
||||||
|
<div class="camera-label">🎥 AGV 摄像头</div>
|
||||||
|
<img :src="agvPreviewUrl" @error="onAgvPreviewError" class="camera-img">
|
||||||
|
</div>
|
||||||
|
<div class="camera-box">
|
||||||
|
<div class="camera-label">🦾 机械臂摄像头</div>
|
||||||
|
<img :src="armPreviewUrl" @error="onArmPreviewError" class="camera-img">
|
||||||
|
</div>
|
||||||
</div>
|
</div>
|
||||||
</section>
|
</section>
|
||||||
|
|
||||||
@@ -113,10 +160,35 @@
|
|||||||
</div>
|
</div>
|
||||||
</div>
|
</div>
|
||||||
|
|
||||||
|
<!-- 错误弹窗 -->
|
||||||
|
<div class="modal-overlay" v-if="waitingError">
|
||||||
|
<div class="modal">
|
||||||
|
<h3>⚠️ 执行错误</h3>
|
||||||
|
<p>[[ errorMsg ]]</p>
|
||||||
|
<div class="modal-actions">
|
||||||
|
<button class="btn btn-warning" @click="skipError">跳过</button>
|
||||||
|
<button class="btn btn-error" @click="abortError">中断</button>
|
||||||
|
</div>
|
||||||
|
</div>
|
||||||
|
</div>
|
||||||
|
|
||||||
|
<!-- 步骤确认弹窗 -->
|
||||||
|
<div class="modal-overlay" v-if="waitingStep">
|
||||||
|
<div class="modal">
|
||||||
|
<h3>🦶 单步执行确认</h3>
|
||||||
|
<p>机器 [[ stepLabel ]] 执行完成，请确认结果是否正确：</p>
|
||||||
|
<div class="modal-actions">
|
||||||
|
<button class="btn btn-success" @click="confirmStep">✅ 正确，继续</button>
|
||||||
|
<button class="btn btn-warning" @click="retryStep">🔄 不正确，重试</button>
|
||||||
|
<button class="btn btn-error" @click="abortStep">⏹️ 中断</button>
|
||||||
|
</div>
|
||||||
|
</div>
|
||||||
|
</div>
|
||||||
|
|
||||||
</main>
|
</main>
|
||||||
</div>
|
</div>
|
||||||
|
|
||||||
<script src="/static/js/vue3.global.prod.js"></script>
|
<script src="/static/js/vue3.global.prod.js"></script>
|
||||||
<script src="/static/js/running.js"></script>
|
<script src="/static/js/running.js?v=20260527b"></script>
|
||||||
</body>
|
</body>
|
||||||
</html>
|
</html>
|
||||||
+82
-5
@@ -10,11 +10,22 @@ createApp({
|
|||||||
progress: 0,
|
progress: 0,
|
||||||
tasks: [],
|
tasks: [],
|
||||||
report: null,
|
report: null,
|
||||||
previewUrl: API + '/api/camera/preview',
|
agvPreviewUrl: API + '/api/camera/preview',
|
||||||
|
armPreviewUrl: API + '/api/camera/arm_refresh',
|
||||||
polling: null,
|
polling: null,
|
||||||
logs: [],
|
logs: [],
|
||||||
showQrModal: false,
|
showQrModal: false,
|
||||||
qrValue: '',
|
qrValue: '',
|
||||||
|
// 错误弹窗 / 单步执行
|
||||||
|
waitingError: false,
|
||||||
|
errorMsg: '',
|
||||||
|
waitingStep: false,
|
||||||
|
stepLabel: '',
|
||||||
|
// 任务步骤控制开关（机械臂初始化并入AGV移动）
|
||||||
|
agvMoveEnabled: true,
|
||||||
|
qrScanEnabled: true,
|
||||||
|
frontPhotoEnabled: true,
|
||||||
|
backPhotoEnabled: true,
|
||||||
}
|
}
|
||||||
},
|
},
|
||||||
computed: {
|
computed: {
|
||||||
@@ -24,7 +35,9 @@ createApp({
|
|||||||
running: '任务运行中',
|
running: '任务运行中',
|
||||||
paused: '已暂停',
|
paused: '已暂停',
|
||||||
completed: '已完成',
|
completed: '已完成',
|
||||||
waiting_qr: '等待输入二维码'
|
waiting_qr: '等待输入二维码',
|
||||||
|
waiting_error: '⚠️ 执行错误',
|
||||||
|
waiting_step: '🦶 等待步骤确认',
|
||||||
}
|
}
|
||||||
return map[this.missionState] || '未知'
|
return map[this.missionState] || '未知'
|
||||||
},
|
},
|
||||||
@@ -52,6 +65,22 @@ createApp({
|
|||||||
this.progress = data.progress || 0
|
this.progress = data.progress || 0
|
||||||
if (data.tasks) this.tasks = data.tasks
|
if (data.tasks) this.tasks = data.tasks
|
||||||
|
|
||||||
|
// 错误弹窗
|
||||||
|
if (data.waiting_error) {
|
||||||
|
this.waitingError = true
|
||||||
|
this.errorMsg = data.error_msg || '任务执行出错'
|
||||||
|
} else {
|
||||||
|
this.waitingError = false
|
||||||
|
}
|
||||||
|
|
||||||
|
// 步骤确认弹窗
|
||||||
|
if (data.waiting_step) {
|
||||||
|
this.waitingStep = true
|
||||||
|
this.stepLabel = data.step_label || ''
|
||||||
|
} else {
|
||||||
|
this.waitingStep = false
|
||||||
|
}
|
||||||
|
|
||||||
// QR 弹窗
|
// QR 弹窗
|
||||||
if (this.missionState === 'waiting_qr' && !this.showQrModal) {
|
if (this.missionState === 'waiting_qr' && !this.showQrModal) {
|
||||||
this.showQrModal = true
|
this.showQrModal = true
|
||||||
@@ -67,7 +96,7 @@ createApp({
|
|||||||
} catch (e) {}
|
} catch (e) {}
|
||||||
},
|
},
|
||||||
async pollLogs() {
|
async pollLogs() {
|
||||||
if (this.missionState !== 'running' && this.missionState !== 'waiting_qr') return
|
if (this.missionState !== 'running' && this.missionState !== 'waiting_qr' && this.missionState !== 'waiting_error' && this.missionState !== 'waiting_step') return
|
||||||
try {
|
try {
|
||||||
const res = await fetch(API + '/api/mission/log')
|
const res = await fetch(API + '/api/mission/log')
|
||||||
const data = await res.json()
|
const data = await res.json()
|
||||||
@@ -87,9 +116,52 @@ createApp({
|
|||||||
this.progress = 0
|
this.progress = 0
|
||||||
this.report = null
|
this.report = null
|
||||||
this.showQrModal = false
|
this.showQrModal = false
|
||||||
await fetch(API + '/api/mission/start', { method: 'POST' })
|
await fetch(API + '/api/mission/start', {
|
||||||
|
method: 'POST',
|
||||||
|
headers: { 'Content-Type': 'application/json' },
|
||||||
|
body: JSON.stringify({
|
||||||
|
agv_move: this.agvMoveEnabled,
|
||||||
|
qr_scan: this.qrScanEnabled,
|
||||||
|
front_photo: this.frontPhotoEnabled,
|
||||||
|
back_photo: this.backPhotoEnabled,
|
||||||
|
})
|
||||||
|
})
|
||||||
this.missionState = 'running'
|
this.missionState = 'running'
|
||||||
},
|
},
|
||||||
|
async startSingleStep() {
|
||||||
|
if (this.missionState !== 'idle') return
|
||||||
|
this.logs = []
|
||||||
|
this.progress = 0
|
||||||
|
this.report = null
|
||||||
|
this.showQrModal = false
|
||||||
|
await fetch(API + '/api/mission/start', {
|
||||||
|
method: 'POST',
|
||||||
|
headers: { 'Content-Type': 'application/json' },
|
||||||
|
body: JSON.stringify({ single_step: true })
|
||||||
|
})
|
||||||
|
if (this.polling) clearInterval(this.polling)
|
||||||
|
this.poll()
|
||||||
|
},
|
||||||
|
async skipError() {
|
||||||
|
await fetch(API + '/api/mission/error-skip', { method: 'POST' })
|
||||||
|
this.waitingError = false
|
||||||
|
},
|
||||||
|
async abortError() {
|
||||||
|
await fetch(API + '/api/mission/error-abort', { method: 'POST' })
|
||||||
|
this.waitingError = false
|
||||||
|
},
|
||||||
|
async confirmStep() {
|
||||||
|
await fetch(API + '/api/mission/singlestep/confirm', { method: 'POST' })
|
||||||
|
this.waitingStep = false
|
||||||
|
},
|
||||||
|
async retryStep() {
|
||||||
|
await fetch(API + '/api/mission/singlestep/retry', { method: 'POST' })
|
||||||
|
this.waitingStep = false
|
||||||
|
},
|
||||||
|
async abortStep() {
|
||||||
|
await fetch(API + '/api/mission/singlestep/abort', { method: 'POST' })
|
||||||
|
this.waitingStep = false
|
||||||
|
},
|
||||||
async pauseMission() {
|
async pauseMission() {
|
||||||
await fetch(API + '/api/mission/pause', { method: 'POST' })
|
await fetch(API + '/api/mission/pause', { method: 'POST' })
|
||||||
this.missionState = 'paused'
|
this.missionState = 'paused'
|
||||||
@@ -103,6 +175,8 @@ createApp({
|
|||||||
await fetch(API + '/api/mission/stop', { method: 'POST' })
|
await fetch(API + '/api/mission/stop', { method: 'POST' })
|
||||||
this.missionState = 'idle'
|
this.missionState = 'idle'
|
||||||
this.showQrModal = false
|
this.showQrModal = false
|
||||||
|
this.waitingError = false
|
||||||
|
this.waitingStep = false
|
||||||
},
|
},
|
||||||
async submitQr() {
|
async submitQr() {
|
||||||
const val = this.qrValue.trim()
|
const val = this.qrValue.trim()
|
||||||
@@ -123,7 +197,10 @@ createApp({
|
|||||||
body: JSON.stringify({ qr: 'SKIP' })
|
body: JSON.stringify({ qr: 'SKIP' })
|
||||||
})
|
})
|
||||||
},
|
},
|
||||||
onPreviewError(e) {
|
onAgvPreviewError(e) {
|
||||||
|
e.target.style.display = 'none'
|
||||||
|
},
|
||||||
|
onArmPreviewError(e) {
|
||||||
e.target.style.display = 'none'
|
e.target.style.display = 'none'
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
+21
-114
@@ -4,7 +4,7 @@
|
|||||||
<meta charset="UTF-8">
|
<meta charset="UTF-8">
|
||||||
<meta name="viewport" content="width=device-width, initial-scale=1.0">
|
<meta name="viewport" content="width=device-width, initial-scale=1.0">
|
||||||
<title>设置 - AGV 拍摄系统</title>
|
<title>设置 - AGV 拍摄系统</title>
|
||||||
<link rel="stylesheet" href="/static/css/style.css?v=20260520h">
|
<link rel="stylesheet" href="/static/css/style.css?v=20260527a">
|
||||||
</head>
|
</head>
|
||||||
<body>
|
<body>
|
||||||
<div id="app">
|
<div id="app">
|
||||||
@@ -12,7 +12,7 @@
|
|||||||
<div class="logo">⚙️ 系统设置</div>
|
<div class="logo">⚙️ 系统设置</div>
|
||||||
<nav class="nav">
|
<nav class="nav">
|
||||||
<a href="/" class="nav-link">🏠 首页</a>
|
<a href="/" class="nav-link">🏠 首页</a>
|
||||||
<href="/setting" class="nav-link active">⚙️ 设置</a>
|
<a href="/setting" class="nav-link active">⚙️ 设置</a>
|
||||||
<a href="/running" class="nav-link">▶️ 运行</a>
|
<a href="/running" class="nav-link">▶️ 运行</a>
|
||||||
</nav>
|
</nav>
|
||||||
</header>
|
</header>
|
||||||
@@ -135,7 +135,7 @@
|
|||||||
<!-- 正面姿态 -->
|
<!-- 正面姿态 -->
|
||||||
<div style="padding:16px;background:#0f1923">
|
<div style="padding:16px;background:#0f1923">
|
||||||
<h4 style="margin:0 0 12px 0;color:#388e3c">🟢 正面姿态</h4>
|
<h4 style="margin:0 0 12px 0;color:#388e3c">🟢 正面姿态</h4>
|
||||||
<div v-for="pose in m.poses.filter(p => p.photo_type === 'front')" :key="pose.id" style="background:#0f1923;padding:12px;border:1px solid #2a3441;border-radius:6px;margin-bottom:8px">
|
<div v-for="pose in (m.poses || []).filter(p => p.photo_type === 'front')" :key="pose.id" style="background:#0f1923;padding:12px;border:1px solid #2a3441;border-radius:6px;margin-bottom:8px">
|
||||||
<div style="display:flex;justify-content:space-between;align-items:center;margin-bottom:8px">
|
<div style="display:flex;justify-content:space-between;align-items:center;margin-bottom:8px">
|
||||||
<strong>{% raw %}{{ pose.name || '正面姿态' }}{% endraw %}</strong>
|
<strong>{% raw %}{{ pose.name || '正面姿态' }}{% endraw %}</strong>
|
||||||
<button class="btn btn-danger btn-small" @click="deletePose(m.id, pose.id)">删除</button>
|
<button class="btn btn-danger btn-small" @click="deletePose(m.id, pose.id)">删除</button>
|
||||||
@@ -182,7 +182,7 @@
|
|||||||
<!-- 背面姿态 -->
|
<!-- 背面姿态 -->
|
||||||
<div style="padding:16px;background:#0d1420">
|
<div style="padding:16px;background:#0d1420">
|
||||||
<h4 style="margin:0 0 12px 0;color:#d32f2f">🔴 背面姿态</h4>
|
<h4 style="margin:0 0 12px 0;color:#d32f2f">🔴 背面姿态</h4>
|
||||||
<div v-for="pose in m.poses.filter(p => p.photo_type === 'back')" :key="pose.id" style="background:#0f1923;padding:12px;border:1px solid #2a3441;border-radius:6px;margin-bottom:8px">
|
<div v-for="pose in (m.poses || []).filter(p => p.photo_type === 'back')" :key="pose.id" style="background:#0f1923;padding:12px;border:1px solid #2a3441;border-radius:6px;margin-bottom:8px">
|
||||||
<div style="display:flex;justify-content:space-between;align-items:center;margin-bottom:8px">
|
<div style="display:flex;justify-content:space-between;align-items:center;margin-bottom:8px">
|
||||||
<strong>{% raw %}{{ pose.name || '背面姿态' }}{% endraw %}</strong>
|
<strong>{% raw %}{{ pose.name || '背面姿态' }}{% endraw %}</strong>
|
||||||
<button class="btn btn-danger btn-small" @click="deletePose(m.id, pose.id)">删除</button>
|
<button class="btn btn-danger btn-small" @click="deletePose(m.id, pose.id)">删除</button>
|
||||||
@@ -268,6 +268,7 @@
|
|||||||
<button class="btn btn-primary" @click="generateGrid">🔲 生成网格</button>
|
<button class="btn btn-primary" @click="generateGrid">🔲 生成网格</button>
|
||||||
<button class="btn btn-secondary" @click="saveMissionConfig">💾 保存网格</button>
|
<button class="btn btn-secondary" @click="saveMissionConfig">💾 保存网格</button>
|
||||||
<button class="btn btn-warning" @click="initPose" :disabled="initPoseLoading">📍 初始化位置</button>
|
<button class="btn btn-warning" @click="initPose" :disabled="initPoseLoading">📍 初始化位置</button>
|
||||||
|
<button class="btn btn-primary" @click="goToOrigin">🏠 回到原点</button>
|
||||||
</div>
|
</div>
|
||||||
</div>
|
</div>
|
||||||
|
|
||||||
@@ -348,116 +349,22 @@
|
|||||||
</div>
|
</div>
|
||||||
</section>
|
</section>
|
||||||
|
|
||||||
<!-- 中：选中机器的配置 -->
|
<!-- 机械臂初始姿态 -->
|
||||||
<section class="card" v-if="selectedMachine && selectedMachine.front && selectedMachine.back" style="margin-top:16px">
|
<section class="card" style="margin-top:16px">
|
||||||
<h2>② 点位配置 — 第{% raw %}{{ selectedMachine.row+1 }}{% endraw %}行 第{% raw %}{{ selectedMachine.col+1 }}{% endraw %}列 <button class="btn btn-small" @click="clearSelection()">← 返回</button></h2>
|
<h2>② 🦾 机械臂初始姿态</h2>
|
||||||
|
<p class="hint" style="margin-bottom:12px">每个机器执行前恢复的初始姿态（6个关节角度，单位：度）</p>
|
||||||
<!-- 正面点位 -->
|
<div class="form-row" style="flex-wrap:wrap;gap:12px">
|
||||||
<div class="machine-form">
|
<div v-for="j in 6" :key="'armInit'+j" class="form-group" style="min-width:100px">
|
||||||
<h3>📷 正面点位</h3>
|
<label>J{% raw %}{{ j }}{% endraw %}</label>
|
||||||
<div class="form-row">
|
<input type="number" step="0.5"
|
||||||
<div class="form-group">
|
v-model.number="armInitialPose[j-1]"
|
||||||
<label>X 坐标</label>
|
style="width:100%;padding:8px;border:1px solid #2a3441;border-radius:4px">
|
||||||
<input type="number" step="0.01" v-model.number="selectedMachine.front.coords[0]" placeholder="0.00">
|
|
||||||
</div>
|
|
||||||
<div class="form-group">
|
|
||||||
<label>Y 坐标</label>
|
|
||||||
<input type="number" step="0.01" v-model.number="selectedMachine.front.coords[1]" placeholder="0.00">
|
|
||||||
</div>
|
|
||||||
<div class="form-group">
|
|
||||||
<label>Yaw (弧度)</label>
|
|
||||||
<input type="number" step="0.01" v-model.number="selectedMachine.front.coords[2]" placeholder="0.00">
|
|
||||||
</div>
|
|
||||||
<div class="form-group" style="align-self:end">
|
|
||||||
<button class="btn btn-small btn-primary" @click="readPosition('front')" :disabled="!agvConnected">📍 读取当前位置</button>
|
|
||||||
</div>
|
|
||||||
</div>
|
</div>
|
||||||
<!-- 正面姿态列表 -->
|
<div class="form-group" style="align-self:end">
|
||||||
<div v-if="selectedMachine.front.poses && selectedMachine.front.poses.length > 0" class="pose-list">
|
<button class="btn btn-primary" @click="saveArmInitialPose">💾 保存初始姿态</button>
|
||||||
<h4>正面姿态 ({% raw %}{{ selectedMachine.front.poses.length }}{% endraw %} 个)</h4>
|
<button class="btn btn-secondary" @click="loadArmCurrentAngles" :disabled="!armConnected" style="margin-left:6px">📋 读取当前角度</button>
|
||||||
<div v-for="pose in selectedMachine.front.poses" :key="pose.id" class="pose-item">
|
<button class="btn btn-primary" @click="applyArmInitialPose" :disabled="!armConnected" style="margin-left:6px">🎯 应用当前姿态</button>
|
||||||
<span class="pose-name">{% raw %}{{ pose.name }}{% endraw %}</span>
|
|
||||||
<span class="pose-angles" v-if="pose.arm_angles">角度: {% raw %}{{ formatAngles(pose.arm_angles) }}{% endraw %}</span>
|
|
||||||
<button class="btn-icon" @click="deletePose(selectedMachine.id, 'front', pose.id)">🗑️</button>
|
|
||||||
</div>
|
|
||||||
</div>
|
</div>
|
||||||
<div class="pose-add">
|
|
||||||
<input type="text" v-model="poseForm.name" placeholder="姿态名称（如：正面全景）">
|
|
||||||
<button class="btn btn-small btn-success" @click="addPoseToMachine(selectedMachine.id, 'front')">➕ 添加姿态</button>
|
|
||||||
</div>
|
|
||||||
</div>
|
|
||||||
|
|
||||||
<!-- 背面点位 -->
|
|
||||||
<div class="machine-form" style="margin-top:16px">
|
|
||||||
<h3>📷 背面点位</h3>
|
|
||||||
<div class="form-row">
|
|
||||||
<div class="form-group">
|
|
||||||
<label>X 坐标</label>
|
|
||||||
<input type="number" step="0.01" v-model.number="selectedMachine.back.coords[0]" placeholder="0.00">
|
|
||||||
</div>
|
|
||||||
<div class="form-group">
|
|
||||||
<label>Y 坐标</label>
|
|
||||||
<input type="number" step="0.01" v-model.number="selectedMachine.back.coords[1]" placeholder="0.00">
|
|
||||||
</div>
|
|
||||||
<div class="form-group">
|
|
||||||
<label>Yaw (弧度)</label>
|
|
||||||
<input type="number" step="0.01" v-model.number="selectedMachine.back.coords[2]" placeholder="0.00">
|
|
||||||
</div>
|
|
||||||
<div class="form-group" style="align-self:end">
|
|
||||||
<button class="btn btn-small btn-primary" @click="readPosition('back')" :disabled="!agvConnected">📍 读取当前位置</button>
|
|
||||||
</div>
|
|
||||||
</div>
|
|
||||||
<!-- 背面姿态列表 -->
|
|
||||||
<div v-if="selectedMachine.back.poses && selectedMachine.back.poses.length > 0" class="pose-list">
|
|
||||||
<h4>背面姿态 ({% raw %}{{ selectedMachine.back.poses.length }}{% endraw %} 个)</h4>
|
|
||||||
<div v-for="pose in selectedMachine.back.poses" :key="pose.id" class="pose-item">
|
|
||||||
<span class="pose-name">{% raw %}{{ pose.name }}{% endraw %}</span>
|
|
||||||
<span class="pose-angles" v-if="pose.arm_angles">角度: {% raw %}{{ formatAngles(pose.arm_angles) }}{% endraw %}</span>
|
|
||||||
<button class="btn-icon" @click="deletePose(selectedMachine.id, 'back', pose.id)">🗑️</button>
|
|
||||||
</div>
|
|
||||||
</div>
|
|
||||||
<div class="pose-add">
|
|
||||||
<input type="text" v-model="poseForm.name" placeholder="姿态名称（如：背面细节）">
|
|
||||||
<button class="btn btn-small btn-success" @click="addPoseToMachine(selectedMachine.id, 'back')">➕ 添加姿态</button>
|
|
||||||
</div>
|
|
||||||
</div>
|
|
||||||
|
|
||||||
<!-- 二维码位置 -->
|
|
||||||
<div class="machine-form" style="margin-top:16px" v-if="hasQr">
|
|
||||||
<h3>🔍 二维码位置</h3>
|
|
||||||
<div class="form-row">
|
|
||||||
<div class="form-group">
|
|
||||||
<label>X 坐标</label>
|
|
||||||
<input type="number" step="0.01" :value="safeQrCoord(0)" @input="setQrCoord(0, Number($event.target.value))" placeholder="0.00">
|
|
||||||
</div>
|
|
||||||
<div class="form-group">
|
|
||||||
<label>Y 坐标</label>
|
|
||||||
<input type="number" step="0.01" :value="safeQrCoord(1)" @input="setQrCoord(1, Number($event.target.value))" placeholder="0.00">
|
|
||||||
</div>
|
|
||||||
<div class="form-group">
|
|
||||||
<label>Yaw (弧度)</label>
|
|
||||||
<input type="number" step="0.01" :value="safeQrCoord(2)" @input="setQrCoord(2, Number($event.target.value))" placeholder="0.00">
|
|
||||||
</div>
|
|
||||||
<div class="form-group" style="align-self:end">
|
|
||||||
<button class="btn btn-small btn-primary" @click="readQRPosition" :disabled="!agvConnected">📍 读取当前位置</button>
|
|
||||||
</div>
|
|
||||||
</div>
|
|
||||||
<!-- QR 扫描结果 -->
|
|
||||||
<div v-if="hasQrValue" style="margin-top:8px;padding:8px;background:#0f1923;border-radius:6px">
|
|
||||||
<span style="font-size:13px">📱 二维码值: <strong>{% raw %}{{ safeQr('qr_value') }}{% endraw %}</strong></span>
|
|
||||||
<span v-if="hasQrModelId" style="margin-left:12px;font-size:13px">🏷️ 匹配机型: <strong>{% raw %}{{ safeQrModelName() }}{% endraw %}</strong></span>
|
|
||||||
<span v-else style="margin-left:12px;font-size:13px;color:#ff9800">⚠️ 未匹配到机型</span>
|
|
||||||
</div>
|
|
||||||
<div class="btn-row" style="margin-top:8px">
|
|
||||||
<button class="btn btn-small btn-success" @click="scanQRCode(selectedMachine.id)" :disabled="!cameraOpened || qrScanning">
|
|
||||||
{% raw %}{{ qrScanning ? '扫描中...' : '📷 扫描二维码' }}{% endraw %}
|
|
||||||
</button>
|
|
||||||
</div>
|
|
||||||
</div>
|
|
||||||
|
|
||||||
<div class="btn-row" style="margin-top:16px">
|
|
||||||
<button class="btn btn-danger" @click="deleteMachine(selectedMachine.id)">🗑️ 删除此机器</button>
|
|
||||||
<button class="btn btn-secondary" @click="saveMachineCoords">💾 保存此机器配置</button>
|
|
||||||
</div>
|
</div>
|
||||||
</section>
|
</section>
|
||||||
|
|
||||||
@@ -676,7 +583,7 @@
|
|||||||
</main>
|
</main>
|
||||||
</div>
|
</div>
|
||||||
|
|
||||||
<script src="/static/js/vue3.global.prod.js?v=20260520h"></script>
|
<script src="/static/js/vue3.global.prod.js?v=20260527a"></script>
|
||||||
<script src="/static/js/setting.js?v=20260520h"></script>
|
<script src="/static/js/setting.js?v=20260527a"></script>
|
||||||
</body>
|
</body>
|
||||||
</html>
|
</html>
|
||||||
|
|||||||
+210
-8
@@ -26,6 +26,9 @@ createApp({
|
|||||||
newPointName: '',
|
newPointName: '',
|
||||||
newPointMode: 'front',
|
newPointMode: 'front',
|
||||||
newPointSequence: ['front', 'back'],
|
newPointSequence: ['front', 'back'],
|
||||||
|
// 点位编辑器弹窗
|
||||||
|
editingPoint: null,
|
||||||
|
pointEditor: { x: 0, y: 0, yaw: 0 },
|
||||||
// 机型（姿态组）
|
// 机型（姿态组）
|
||||||
models: [],
|
models: [],
|
||||||
selectedModelId: null,
|
selectedModelId: null,
|
||||||
@@ -56,6 +59,7 @@ createApp({
|
|||||||
qrScanningId: null,
|
qrScanningId: null,
|
||||||
armCameraUrl: API + '/api/camera/arm_refresh',
|
armCameraUrl: API + '/api/camera/arm_refresh',
|
||||||
newQrName: '',
|
newQrName: '',
|
||||||
|
armInitialPose: [0, 0, 0, 0, 0, 0],
|
||||||
}
|
}
|
||||||
},
|
},
|
||||||
mounted() {
|
mounted() {
|
||||||
@@ -158,20 +162,41 @@ createApp({
|
|||||||
if (res.ok) {
|
if (res.ok) {
|
||||||
const data = await res.json()
|
const data = await res.json()
|
||||||
this.nav2Available = data.nav2_available
|
this.nav2Available = data.nav2_available
|
||||||
if (data.current_pos) {
|
if (data.current_position) {
|
||||||
this.navCurrentPos = data.current_pos
|
this.navCurrentPos = data.current_position
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
} catch (e) {}
|
} catch (e) {}
|
||||||
},
|
},
|
||||||
async onMapClick(e) {
|
async onMapClick(e) {
|
||||||
if (!this.mapMeta || !this.agvConnected) return
|
if (!this.mapMeta) {
|
||||||
|
this.mapMsg = '❌ 地图未加载'
|
||||||
|
setTimeout(() => { this.mapMsg = '' }, 3000)
|
||||||
|
return
|
||||||
|
}
|
||||||
|
if (!this.agvConnected) {
|
||||||
|
this.mapMsg = '❌ AGV 未连接，无法导航'
|
||||||
|
setTimeout(() => { this.mapMsg = '' }, 3000)
|
||||||
|
return
|
||||||
|
}
|
||||||
const rect = e.target.getBoundingClientRect()
|
const rect = e.target.getBoundingClientRect()
|
||||||
const px = (e.clientX - rect.left) / rect.width
|
let px = (e.clientX - rect.left) / rect.width
|
||||||
const py = (e.clientY - rect.top) / rect.height
|
let py = (e.clientY - rect.top) / rect.height
|
||||||
|
// 逆旋转补偿：地图 CSS transform: rotate() 后，点击坐标需反向旋转
|
||||||
|
// 使同一物理点在不同旋转角度下返回相同的世界坐标
|
||||||
|
const rotation = (this.mapRotation || 0) * Math.PI / 180
|
||||||
|
if (rotation !== 0) {
|
||||||
|
const cx = px - 0.5
|
||||||
|
const cy = py - 0.5
|
||||||
|
const cos = Math.cos(-rotation)
|
||||||
|
const sin = Math.sin(-rotation)
|
||||||
|
px = cx * cos - cy * sin + 0.5
|
||||||
|
py = cx * sin + cy * cos + 0.5
|
||||||
|
}
|
||||||
const { resolution, origin } = this.mapMeta
|
const { resolution, origin } = this.mapMeta
|
||||||
const wx = origin[0] + px * resolution * this.mapMeta.width
|
const wx = origin[0] + px * resolution * this.mapMeta.width
|
||||||
const wy = origin[1] + (1 - py) * resolution * this.mapMeta.height
|
const wy = origin[1] + (1 - py) * resolution * this.mapMeta.height
|
||||||
|
if (!confirm(`是否导航到该坐标？\nX: ${wx.toFixed(3)}\nY: ${wy.toFixed(3)}`)) return
|
||||||
try {
|
try {
|
||||||
const res = await fetch(API + '/api/navigate/to', {
|
const res = await fetch(API + '/api/navigate/to', {
|
||||||
method: 'POST',
|
method: 'POST',
|
||||||
@@ -254,6 +279,129 @@ createApp({
|
|||||||
if (!angles) return '—'
|
if (!angles) return '—'
|
||||||
return angles.map(a => (a || 0).toFixed(1) + '°').join(' / ')
|
return angles.map(a => (a || 0).toFixed(1) + '°').join(' / ')
|
||||||
},
|
},
|
||||||
|
// === 点位编辑器弹窗 ===
|
||||||
|
openPointEdit(ri, ci) {
|
||||||
|
const point = this.getPointAt(ri, ci)
|
||||||
|
this.editingPoint = { pointRow: ri, col: ci }
|
||||||
|
if (point && point.coords && point.coords.length >= 3) {
|
||||||
|
this.pointEditor = { x: point.coords[0], y: point.coords[1], yaw: point.coords[2] || 0 }
|
||||||
|
} else {
|
||||||
|
this.pointEditor = { x: 0, y: 0, yaw: 0 }
|
||||||
|
}
|
||||||
|
},
|
||||||
|
closePointEdit() {
|
||||||
|
this.editingPoint = null
|
||||||
|
},
|
||||||
|
getPointOwnerLabel(pointRow, col) {
|
||||||
|
const rows = this.missionConfig.rows || 0
|
||||||
|
if (pointRow === 0) {
|
||||||
|
return `机器行1·正面`
|
||||||
|
} else if (pointRow >= rows) {
|
||||||
|
return `机器行${rows}·背面`
|
||||||
|
} else {
|
||||||
|
return `机器行${pointRow}·背面 + 机器行${pointRow+1}·正面`
|
||||||
|
}
|
||||||
|
},
|
||||||
|
async loadPointFromAgv() {
|
||||||
|
try {
|
||||||
|
const res = await fetch(API + '/api/agv/position')
|
||||||
|
const data = await res.json()
|
||||||
|
if (data.ok && data.position && data.position.length >= 3) {
|
||||||
|
this.pointEditor.x = data.position[0] || 0
|
||||||
|
this.pointEditor.y = data.position[1] || 0
|
||||||
|
this.pointEditor.yaw = data.position[2] || 0
|
||||||
|
} else {
|
||||||
|
alert('读取AGV位置失败')
|
||||||
|
}
|
||||||
|
} catch (e) { alert('读取AGV位置失败: ' + e.message) }
|
||||||
|
},
|
||||||
|
async savePoint() {
|
||||||
|
if (!this.editingPoint) return
|
||||||
|
const { pointRow, col } = this.editingPoint
|
||||||
|
const coords = [this.pointEditor.x, this.pointEditor.y, this.pointEditor.yaw]
|
||||||
|
const rows = this.missionConfig.rows || 0
|
||||||
|
// 根据点位行确定 side
|
||||||
|
const sides = []
|
||||||
|
if (pointRow === 0) {
|
||||||
|
sides.push('front')
|
||||||
|
} else if (pointRow >= rows) {
|
||||||
|
sides.push('back')
|
||||||
|
} else {
|
||||||
|
sides.push('back')
|
||||||
|
sides.push('front')
|
||||||
|
}
|
||||||
|
try {
|
||||||
|
for (const side of sides) {
|
||||||
|
const res = await fetch(API + '/api/mission/positions', {
|
||||||
|
method: 'POST',
|
||||||
|
headers: { 'Content-Type': 'application/json' },
|
||||||
|
body: JSON.stringify({ row: pointRow, col, side, coords, poses: [] })
|
||||||
|
})
|
||||||
|
const data = await res.json()
|
||||||
|
if (!data.ok) { alert(`保存失败(${side}): ` + (data.error || '')); return }
|
||||||
|
}
|
||||||
|
alert('点位已保存')
|
||||||
|
await this.loadMissionConfig()
|
||||||
|
this.closePointEdit()
|
||||||
|
} catch (e) { alert('保存失败: ' + e.message) }
|
||||||
|
},
|
||||||
|
async navigateToPoint() {
|
||||||
|
if (!confirm(`确认导航到该点位?\nX: ${this.pointEditor.x} Y: ${this.pointEditor.y} Yaw: ${this.pointEditor.yaw}`)) return
|
||||||
|
try {
|
||||||
|
const res = await fetch(API + '/api/navigate/to', {
|
||||||
|
method: 'POST',
|
||||||
|
headers: { 'Content-Type': 'application/json' },
|
||||||
|
body: JSON.stringify({
|
||||||
|
x: this.pointEditor.x,
|
||||||
|
y: this.pointEditor.y,
|
||||||
|
yaw: this.pointEditor.yaw
|
||||||
|
})
|
||||||
|
})
|
||||||
|
const data = await res.json()
|
||||||
|
if (!data.ok) { alert('导航失败: ' + (data.error || '')) }
|
||||||
|
} catch (e) { alert('导航失败: ' + e.message) }
|
||||||
|
},
|
||||||
|
async goToOrigin() {
|
||||||
|
if (!confirm('确认导航到原点 (0, 0, 0)?')) return
|
||||||
|
try {
|
||||||
|
const res = await fetch(API + '/api/navigate/to', {
|
||||||
|
method: 'POST',
|
||||||
|
headers: { 'Content-Type': 'application/json' },
|
||||||
|
body: JSON.stringify({ x: 0, y: 0, yaw: 0 })
|
||||||
|
})
|
||||||
|
const data = await res.json()
|
||||||
|
if (data.ok) {
|
||||||
|
this.mapMsg = '✅ 已发送导航到原点'
|
||||||
|
} else {
|
||||||
|
this.mapMsg = '❌ ' + (data.error || '导航失败')
|
||||||
|
}
|
||||||
|
} catch (e) {
|
||||||
|
this.mapMsg = '❌ 导航请求失败: ' + e.message
|
||||||
|
}
|
||||||
|
setTimeout(() => { this.mapMsg = '' }, 3000)
|
||||||
|
},
|
||||||
|
async clearPoint() {
|
||||||
|
if (!this.editingPoint) return
|
||||||
|
const { pointRow, col } = this.editingPoint
|
||||||
|
const rows = this.missionConfig.rows || 0
|
||||||
|
const sides = pointRow === 0 ? ['front'] : pointRow >= rows ? ['back'] : ['front', 'back']
|
||||||
|
try {
|
||||||
|
for (const side of sides) {
|
||||||
|
await fetch(API + '/api/mission/positions', {
|
||||||
|
method: 'POST',
|
||||||
|
headers: { 'Content-Type': 'application/json' },
|
||||||
|
body: JSON.stringify({ row: pointRow, col, side, coords: [0, 0, 0], poses: [] })
|
||||||
|
})
|
||||||
|
}
|
||||||
|
await this.loadMissionConfig()
|
||||||
|
this.closePointEdit()
|
||||||
|
} catch (e) { alert('清空失败: ' + e.message) }
|
||||||
|
},
|
||||||
|
canClearPoint(pointRow, col) {
|
||||||
|
const point = this.getPointAt(pointRow, col)
|
||||||
|
if (!point || !point.coords) return true
|
||||||
|
return point.coords[0] === 0 && point.coords[1] === 0
|
||||||
|
},
|
||||||
// === 机型管理 ===
|
// === 机型管理 ===
|
||||||
async loadAllModels() {
|
async loadAllModels() {
|
||||||
const res = await fetch(API + '/api/models/list')
|
const res = await fetch(API + '/api/models/list')
|
||||||
@@ -344,7 +492,7 @@ createApp({
|
|||||||
const res = await fetch(API + '/api/arm/set_angles', {
|
const res = await fetch(API + '/api/arm/set_angles', {
|
||||||
method: 'POST',
|
method: 'POST',
|
||||||
headers: { 'Content-Type': 'application/json' },
|
headers: { 'Content-Type': 'application/json' },
|
||||||
body: JSON.stringify({ angles: pose.arm_angles, speed: 50 })
|
body: JSON.stringify({ angles: pose.arm_angles, speed: 500 })
|
||||||
})
|
})
|
||||||
const data = await res.json()
|
const data = await res.json()
|
||||||
if (data.ok) { alert('姿态已应用到机械臂') }
|
if (data.ok) { alert('姿态已应用到机械臂') }
|
||||||
@@ -382,6 +530,8 @@ createApp({
|
|||||||
this.missionConfig.rows = data.config.rows || 3
|
this.missionConfig.rows = data.config.rows || 3
|
||||||
this.missionConfig.cols = data.config.cols || 3
|
this.missionConfig.cols = data.config.cols || 3
|
||||||
this.missionConfig.grid = data.config.grid || []
|
this.missionConfig.grid = data.config.grid || []
|
||||||
|
this.missionConfig.positions = data.config.positions || []
|
||||||
|
this.armInitialPose = data.config.arm_initial_pose || [0, 0, 0, 0, 0, 0]
|
||||||
}
|
}
|
||||||
} catch (e) { console.error('加载任务配置失败', e) }
|
} catch (e) { console.error('加载任务配置失败', e) }
|
||||||
},
|
},
|
||||||
@@ -413,7 +563,8 @@ createApp({
|
|||||||
body: JSON.stringify({
|
body: JSON.stringify({
|
||||||
rows: this.missionConfig.rows,
|
rows: this.missionConfig.rows,
|
||||||
cols: this.missionConfig.cols,
|
cols: this.missionConfig.cols,
|
||||||
grid: this.missionConfig.grid
|
grid: this.missionConfig.grid,
|
||||||
|
arm_initial_pose: this.armInitialPose
|
||||||
})
|
})
|
||||||
})
|
})
|
||||||
const data = await res.json()
|
const data = await res.json()
|
||||||
@@ -422,6 +573,47 @@ createApp({
|
|||||||
}
|
}
|
||||||
} catch (e) { alert('保存失败: ' + e.message) }
|
} catch (e) { alert('保存失败: ' + e.message) }
|
||||||
},
|
},
|
||||||
|
async saveArmInitialPose() {
|
||||||
|
try {
|
||||||
|
const res = await fetch(API + '/api/mission/config', {
|
||||||
|
method: 'POST',
|
||||||
|
headers: { 'Content-Type': 'application/json' },
|
||||||
|
body: JSON.stringify({
|
||||||
|
rows: this.missionConfig.rows,
|
||||||
|
cols: this.missionConfig.cols,
|
||||||
|
grid: this.missionConfig.grid,
|
||||||
|
arm_initial_pose: this.armInitialPose
|
||||||
|
})
|
||||||
|
})
|
||||||
|
const data = await res.json()
|
||||||
|
if (data.ok) alert('✅ 机械臂初始姿态已保存')
|
||||||
|
else alert('❌ 保存失败')
|
||||||
|
} catch (e) { alert('保存失败: ' + e.message) }
|
||||||
|
},
|
||||||
|
async loadArmCurrentAngles() {
|
||||||
|
if (!this.armConnected) { alert('机械臂未连接'); return }
|
||||||
|
try {
|
||||||
|
const res = await fetch(API + '/api/arm/get_angles')
|
||||||
|
const data = await res.json()
|
||||||
|
if (data.ok && data.angles) {
|
||||||
|
this.armInitialPose = [...data.angles]
|
||||||
|
}
|
||||||
|
} catch (e) { alert('读取角度失败: ' + e.message) }
|
||||||
|
},
|
||||||
|
async applyArmInitialPose() {
|
||||||
|
if (!this.armConnected) { alert('机械臂未连接'); return }
|
||||||
|
if (!confirm('确认应用初始姿态到机械臂？')) return
|
||||||
|
try {
|
||||||
|
const res = await fetch(API + '/api/arm/set_angles', {
|
||||||
|
method: 'POST',
|
||||||
|
headers: { 'Content-Type': 'application/json' },
|
||||||
|
body: JSON.stringify({ angles: this.armInitialPose, speed: 30 })
|
||||||
|
})
|
||||||
|
const data = await res.json()
|
||||||
|
if (data.ok) alert('✅ 机械臂已移动到初始姿态')
|
||||||
|
else alert('❌ 应用失败: ' + (data.error || ''))
|
||||||
|
} catch (e) { alert('应用失败: ' + e.message) }
|
||||||
|
},
|
||||||
async loadAllMachines() {
|
async loadAllMachines() {
|
||||||
try {
|
try {
|
||||||
const res = await fetch(API + '/api/mission/machines')
|
const res = await fetch(API + '/api/mission/machines')
|
||||||
@@ -465,6 +657,16 @@ createApp({
|
|||||||
this.selectMachine(m)
|
this.selectMachine(m)
|
||||||
}
|
}
|
||||||
},
|
},
|
||||||
|
toggleMachine(ri, ci, event) {
|
||||||
|
if (event.target.checked) {
|
||||||
|
// 无机器 → 创建机器
|
||||||
|
this.createMachine(ri, ci)
|
||||||
|
} else {
|
||||||
|
// 有机器 → 删除机器
|
||||||
|
const m = this.getMachineAt(ri, ci)
|
||||||
|
if (m) this.deleteMachine(m.id)
|
||||||
|
}
|
||||||
|
},
|
||||||
async createMachine(ri, ci) {
|
async createMachine(ri, ci) {
|
||||||
try {
|
try {
|
||||||
const machineId = 'm_' + ri + '_' + ci
|
const machineId = 'm_' + ri + '_' + ci
|
||||||
@@ -887,7 +1089,7 @@ createApp({
|
|||||||
const res = await fetch(API + '/api/arm/set_angles', {
|
const res = await fetch(API + '/api/arm/set_angles', {
|
||||||
method: 'POST',
|
method: 'POST',
|
||||||
headers: {'Content-Type':'application/json'},
|
headers: {'Content-Type':'application/json'},
|
||||||
body: JSON.stringify({ angles: q.joint_angles, speed: 50 })
|
body: JSON.stringify({ angles: q.joint_angles, speed: 500 })
|
||||||
})
|
})
|
||||||
const data = await res.json()
|
const data = await res.json()
|
||||||
if (data.ok) { alert('姿态已应用到机械臂')
|
if (data.ok) { alert('姿态已应用到机械臂')
|
||||||
|
|||||||
@@ -4,7 +4,7 @@
|
|||||||
<meta charset="UTF-8">
|
<meta charset="UTF-8">
|
||||||
<meta name="viewport" content="width=device-width, initial-scale=1.0">
|
<meta name="viewport" content="width=device-width, initial-scale=1.0">
|
||||||
<title>设置 - AGV 拍摄系统</title>
|
<title>设置 - AGV 拍摄系统</title>
|
||||||
<link rel="stylesheet" href="/static/css/style.css?v=20260527a">
|
<link rel="stylesheet" href="/static/css/style.css?v=20260526a">
|
||||||
</head>
|
</head>
|
||||||
<body>
|
<body>
|
||||||
<div id="app">
|
<div id="app">
|
||||||
@@ -583,7 +583,7 @@
|
|||||||
</main>
|
</main>
|
||||||
</div>
|
</div>
|
||||||
|
|
||||||
<script src="/static/js/vue3.global.prod.js?v=20260527a"></script>
|
<script src="/static/js/vue3.global.prod.js?v=20260526a"></script>
|
||||||
<script src="/static/js/setting.js?v=20260527a"></script>
|
<script src="/static/js/setting.js?v=20260526f"></script>
|
||||||
</body>
|
</body>
|
||||||
</html>
|
</html>
|
||||||
|
|||||||
+182
-126
@@ -185,31 +185,43 @@ class MissionExecutorV3:
|
|||||||
# 1. 生成蛇形路径
|
# 1. 生成蛇形路径
|
||||||
path = MissionExecutorV3._build_snake_path(rows, cols, grid)
|
path = MissionExecutorV3._build_snake_path(rows, cols, grid)
|
||||||
if not path:
|
if not path:
|
||||||
self._log("❌ 网格中没有机器，任务终止")
|
self._log("❌ 网格中没有点位，任务终止")
|
||||||
self.report["error"] = "No machines in grid"
|
self.report["error"] = "No points in grid"
|
||||||
return self._finish(0)
|
return self._finish(0)
|
||||||
|
|
||||||
self.report["total"] = len(path)
|
# 统计总机器数（用于进度计算）
|
||||||
self._log(f"📍 蛇形路径生成: {len(path)} 台机器")
|
total_machines = 0
|
||||||
|
for rw in grid:
|
||||||
|
for v in rw:
|
||||||
|
if v:
|
||||||
|
total_machines += 1
|
||||||
|
self.report["total"] = total_machines
|
||||||
|
# 扫码结果缓存：正面扫到的 QR 传给背面
|
||||||
|
qr_cache = {}
|
||||||
|
|
||||||
# 初始化任务列表
|
# 初始化任务列表（机器级别）
|
||||||
self.report["tasks"] = [{
|
self.report["tasks"] = []
|
||||||
"row": r, "col": c,
|
for r in range(rows):
|
||||||
"machine_id": f"m_{r}_{c}",
|
for c in range(cols):
|
||||||
"label": f"{r+1}-{c+1}",
|
if MissionExecutorV3._has_machine(grid, r, c):
|
||||||
"status": "pending",
|
self.report["tasks"].append({
|
||||||
"step": "等待",
|
"row": r, "col": c,
|
||||||
"qr_value": None,
|
"machine_id": f"m_{r}_{c}",
|
||||||
"photos_front": 0,
|
"label": f"{r+1}-{c+1}",
|
||||||
"photos_back": 0,
|
"status": "pending",
|
||||||
} for (r, c) in path]
|
"step": "等待",
|
||||||
|
"qr_value": None,
|
||||||
|
"photos_front": 0,
|
||||||
|
"photos_back": 0,
|
||||||
|
})
|
||||||
|
|
||||||
|
self._log(f"📍 点位蛇形路径: {len(path)} 个点位, {total_machines} 台机器")
|
||||||
|
|
||||||
# 任务步骤控制开关
|
# 任务步骤控制开关
|
||||||
if options is None:
|
if options is None:
|
||||||
options = {}
|
options = {}
|
||||||
opt_arm_init = options.get("arm_init", True)
|
opt_arm_init = options.get("arm_init", True)
|
||||||
opt_agv_move = options.get("agv_move", True)
|
opt_agv_move = options.get("agv_move", True)
|
||||||
# 机械臂初始化并入 AGV 移动：只要开移动，就先初始化机械臂
|
|
||||||
if opt_agv_move:
|
if opt_agv_move:
|
||||||
opt_arm_init = True
|
opt_arm_init = True
|
||||||
opt_qr_scan = options.get("qr_scan", True)
|
opt_qr_scan = options.get("qr_scan", True)
|
||||||
@@ -222,140 +234,151 @@ class MissionExecutorV3:
|
|||||||
arm_initial_pose = mission_config.get("arm_initial_pose", [0.0] * 6)
|
arm_initial_pose = mission_config.get("arm_initial_pose", [0.0] * 6)
|
||||||
has_arm_pose = self.arm_client and any(abs(a) > 0.01 for a in arm_initial_pose)
|
has_arm_pose = self.arm_client and any(abs(a) > 0.01 for a in arm_initial_pose)
|
||||||
|
|
||||||
# 2. 逐台执行
|
# 进度统计
|
||||||
machine_idx = 0
|
max_actions = total_machines * 2 # 每台机器正面+背面
|
||||||
while machine_idx < len(path):
|
completed_actions = 0
|
||||||
|
|
||||||
|
# 2. 逐点位蛇形执行
|
||||||
|
pi = 0
|
||||||
|
while pi < len(path):
|
||||||
if self._stop.is_set():
|
if self._stop.is_set():
|
||||||
self._log("⏹️ 任务已停止")
|
self._log("⏹️ 任务已停止")
|
||||||
break
|
break
|
||||||
|
|
||||||
self._wait_pause()
|
self._wait_pause()
|
||||||
r, c = path[machine_idx]
|
pr, c = path[pi] # pr = 点位行, c = 列
|
||||||
rl, cl = r + 1, c + 1
|
|
||||||
|
|
||||||
# 恢复机械臂初始姿态
|
# 判断该点位需要做什么
|
||||||
if opt_arm_init and has_arm_pose:
|
has_front = pr < rows and MissionExecutorV3._has_machine(grid, pr, c)
|
||||||
|
has_back = pr > 0 and MissionExecutorV3._has_machine(grid, pr - 1, c)
|
||||||
|
|
||||||
|
if not has_front and not has_back:
|
||||||
|
pi += 1
|
||||||
|
continue
|
||||||
|
|
||||||
|
# 日志 & 步骤更新
|
||||||
|
rl_front = pr + 1 if has_front else 0
|
||||||
|
cl_front = c + 1 if has_front else 0
|
||||||
|
rl_back = pr if has_back else 0
|
||||||
|
cl_back = c + 1 if has_back else 0
|
||||||
|
|
||||||
|
log_parts = []
|
||||||
|
if has_front:
|
||||||
|
log_parts.append(f"正面:机器{rl_front}-{cl_front}")
|
||||||
|
task = self._get_task(pr, c)
|
||||||
|
if task:
|
||||||
|
task["status"] = "active"
|
||||||
|
task["step"] = "正面扫码"
|
||||||
|
if has_back:
|
||||||
|
log_parts.append(f"背面:机器{rl_back}-{cl_back}")
|
||||||
|
task = self._get_task(pr - 1, c)
|
||||||
|
if task:
|
||||||
|
task["step"] = "背面拍照"
|
||||||
|
self._log(f"📍 点位 ({pr},{c}) → {' & '.join(log_parts)}")
|
||||||
|
self._step(f"点位({pr},{c})")
|
||||||
|
|
||||||
|
# 恢复机械臂初始姿态（AGV 移动前）
|
||||||
|
if opt_arm_init and has_arm_pose and opt_agv_move:
|
||||||
self._log(" 🦾 恢复机械臂初始姿态")
|
self._log(" 🦾 恢复机械臂初始姿态")
|
||||||
try:
|
try:
|
||||||
self.arm_client.set_angles(arm_initial_pose, speed=500)
|
self.arm_client.set_angles(arm_initial_pose, speed=500)
|
||||||
time.sleep(2)
|
self._wait_arm_ready(arm_initial_pose)
|
||||||
except Exception as e:
|
except Exception as e:
|
||||||
self._log(f" ⚠️ 机械臂初始化失败: {e}")
|
self._log(f" ⚠️ 机械臂初始化失败: {e}")
|
||||||
|
|
||||||
# 更新任务状态 → 正面开始
|
# 导航到该点位的坐标
|
||||||
task = self._get_task(r, c)
|
|
||||||
if task:
|
|
||||||
task["status"] = "active"
|
|
||||||
task["step"] = "正面扫码"
|
|
||||||
|
|
||||||
machine_id = f"m_{r}_{c}"
|
|
||||||
machine = next((m for m in machines if m.get("id") == machine_id), None)
|
|
||||||
if not machine:
|
|
||||||
self._log(f"⚠️ 机器 {rl}-{cl} 不存在，跳过")
|
|
||||||
machine_idx += 1
|
|
||||||
continue
|
|
||||||
|
|
||||||
# --- 正面 ---
|
|
||||||
self._log(f"📍 机器 {rl}-{cl} 进入正面点位")
|
|
||||||
self._step(f"机器 {rl}-{cl} 正面")
|
|
||||||
|
|
||||||
# 导航到正面点位
|
|
||||||
if opt_agv_move:
|
if opt_agv_move:
|
||||||
front_pt = self._find_point(positions, r, c, "front")
|
# 找该点位的任意有效坐标（正面/背面坐标相同）
|
||||||
if front_pt and self._has_coords(front_pt):
|
pos = MissionExecutorV3._find_any_position(positions, pr, c)
|
||||||
if not self._navigate(front_pt, "正面"):
|
if pos and MissionExecutorV3._has_coords(pos):
|
||||||
|
if not self._navigate(pos, f"点位({pr},{c})"):
|
||||||
self._log(f"⚠️ 导航失败，尝试继续")
|
self._log(f"⚠️ 导航失败，尝试继续")
|
||||||
choice = self._wait_error(f"机器 {rl}-{cl} 正面导航失败")
|
choice = self._wait_error(f"点位({pr},{c})导航失败")
|
||||||
if choice == "abort":
|
if choice == "abort":
|
||||||
break
|
break
|
||||||
else:
|
else:
|
||||||
self._log(f"⚠️ 无正面点位坐标")
|
self._log(f"⚠️ 点位({pr},{c})无有效坐标")
|
||||||
else:
|
else:
|
||||||
self._log(" ⏭️ 跳过AGV移动（正面）")
|
self._log(" ⏭️ 跳过AGV移动")
|
||||||
|
|
||||||
# 扫描二维码
|
# --- 正面操作（机器 pr,c 的正面） ---
|
||||||
qr_value = None
|
qr_value = None
|
||||||
if opt_qr_scan:
|
if has_front and not self._stop.is_set():
|
||||||
qr_value = self._scan_qr_with_poses(qr_configs)
|
self._wait_pause()
|
||||||
if self._stop.is_set():
|
if opt_qr_scan:
|
||||||
break
|
qr_value = self._scan_qr_with_poses(qr_configs)
|
||||||
else:
|
if self._stop.is_set():
|
||||||
self._log(" ⏭️ 跳过二维码识别")
|
break
|
||||||
|
|
||||||
# 查机型 + 更新任务步骤
|
|
||||||
model_name = self._lookup_model(qr_value)
|
|
||||||
self._log(f" 🏷️ 机型: {model_name}")
|
|
||||||
if task and qr_value:
|
|
||||||
task["qr_value"] = qr_value
|
|
||||||
if task:
|
|
||||||
task["step"] = "正面拍照"
|
|
||||||
|
|
||||||
# 正面拍照
|
|
||||||
if opt_front_photo:
|
|
||||||
model = self._find_model(models, model_name)
|
|
||||||
if model:
|
|
||||||
self._shoot(model, "front", rl, cl, qr_value or "unknown")
|
|
||||||
else:
|
else:
|
||||||
self._log(f" ⚠️ 未找到机型 {model_name}")
|
self._log(" ⏭️ 跳过二维码识别（正面）")
|
||||||
else:
|
qr_cache[(pr, c)] = qr_value
|
||||||
self._log(" ⏭️ 跳过正面拍照")
|
|
||||||
|
|
||||||
self._progress(machine_idx, 1)
|
task = self._get_task(pr, c)
|
||||||
|
if task and qr_value:
|
||||||
|
task["qr_value"] = qr_value
|
||||||
|
if task:
|
||||||
|
task["step"] = "正面拍照"
|
||||||
|
|
||||||
# --- 背面 ---
|
model_name = self._lookup_model(qr_value)
|
||||||
if task:
|
self._log(f" 🏷️ 机型: {model_name}")
|
||||||
task["step"] = "背面拍照"
|
|
||||||
self._log(f"📍 机器 {rl}-{cl} 进入背面点位")
|
|
||||||
self._step(f"机器 {rl}-{cl} 背面")
|
|
||||||
|
|
||||||
# 导航到背面点位
|
if opt_front_photo and not self._stop.is_set():
|
||||||
# 导航到背面点位之前：恢复机械臂初始姿态
|
model = self._find_model(models, model_name)
|
||||||
if opt_agv_move and has_arm_pose:
|
if model:
|
||||||
self._log(" 🦾 恢复机械臂初始姿态（背面移动前）")
|
self._shoot(model, "front", rl_front, cl_front, qr_value or "unknown")
|
||||||
try:
|
else:
|
||||||
self.arm_client.set_angles(arm_initial_pose, speed=500)
|
self._log(f" ⚠️ 未找到机型 {model_name}")
|
||||||
time.sleep(2)
|
|
||||||
except Exception as e:
|
|
||||||
self._log(f" ⚠️ 机械臂初始化失败: {e}")
|
|
||||||
if opt_agv_move:
|
|
||||||
back_pt = self._find_point(positions, r + 1, c, "back")
|
|
||||||
if back_pt and self._has_coords(back_pt):
|
|
||||||
if not self._navigate(back_pt, "背面"):
|
|
||||||
self._log(f"⚠️ 导航失败，尝试继续")
|
|
||||||
choice = self._wait_error(f"机器 {rl}-{cl} 背面导航失败")
|
|
||||||
if choice == "abort":
|
|
||||||
break
|
|
||||||
else:
|
else:
|
||||||
self._log(f"⚠️ 无背面点位坐标")
|
self._log(" ⏭️ 跳过正面拍照")
|
||||||
else:
|
completed_actions += 1
|
||||||
self._log(" ⏭️ 跳过AGV移动（背面）")
|
|
||||||
|
|
||||||
# 背面拍照
|
# --- 背面操作（机器 pr-1,c 的背面） ---
|
||||||
if opt_back_photo:
|
if has_back and not self._stop.is_set():
|
||||||
if model:
|
self._wait_pause()
|
||||||
self._shoot(model, "back", rl, cl, qr_value or "unknown")
|
back_qr = qr_cache.get((pr - 1, c), "unknown")
|
||||||
else:
|
|
||||||
self._log(" ⏭️ 跳过背面拍照")
|
|
||||||
|
|
||||||
# 标记任务完成
|
task = self._get_task(pr - 1, c)
|
||||||
if task:
|
if task:
|
||||||
task["status"] = "completed"
|
task["step"] = "背面拍照"
|
||||||
task["step"] = "完成"
|
|
||||||
self._progress(machine_idx, 2)
|
|
||||||
|
|
||||||
# 单步执行：等待用户确认
|
model_name = self._lookup_model(back_qr)
|
||||||
|
self._log(f" 🏷️ 机型(背面): {model_name}")
|
||||||
|
|
||||||
|
if opt_back_photo and not self._stop.is_set():
|
||||||
|
model = self._find_model(models, model_name)
|
||||||
|
if model:
|
||||||
|
self._shoot(model, "back", rl_back, cl_back, back_qr)
|
||||||
|
else:
|
||||||
|
self._log(f" ⚠️ 未找到机型 {model_name}")
|
||||||
|
else:
|
||||||
|
self._log(" ⏭️ 跳过背面拍照")
|
||||||
|
completed_actions += 1
|
||||||
|
if task:
|
||||||
|
task["status"] = "completed"
|
||||||
|
task["step"] = "完成"
|
||||||
|
|
||||||
|
# 更新进度
|
||||||
|
if max_actions:
|
||||||
|
self.report["progress"] = min(int(completed_actions / max_actions * 100), 99)
|
||||||
|
|
||||||
|
# 单步执行
|
||||||
if single_step and not self._stop.is_set():
|
if single_step and not self._stop.is_set():
|
||||||
choice = self._wait_step_confirm(rl, cl)
|
choice = self._wait_step_confirm(
|
||||||
|
rl_front if has_front else rl_back,
|
||||||
|
cl_front if has_front else cl_back
|
||||||
|
)
|
||||||
if choice == "abort":
|
if choice == "abort":
|
||||||
break
|
break
|
||||||
elif choice == "retry":
|
elif choice == "retry":
|
||||||
if task:
|
if has_front:
|
||||||
task["status"] = "pending"
|
task = self._get_task(pr, c)
|
||||||
task["step"] = "重试开始"
|
if task:
|
||||||
self._progress(machine_idx, 0)
|
task["status"] = "pending"
|
||||||
continue # 不递增 machine_idx，重新执行
|
task["step"] = "重试开始"
|
||||||
|
completed_actions = max(0, completed_actions - 2)
|
||||||
|
continue
|
||||||
|
|
||||||
machine_idx += 1
|
pi += 1
|
||||||
|
|
||||||
# 3. 回到出发点
|
# 3. 回到出发点
|
||||||
if not self._stop.is_set() and opt_agv_move:
|
if not self._stop.is_set() and opt_agv_move:
|
||||||
@@ -391,17 +414,22 @@ class MissionExecutorV3:
|
|||||||
|
|
||||||
@staticmethod
|
@staticmethod
|
||||||
def _build_snake_path(rows: int, cols: int, grid: list) -> list:
|
def _build_snake_path(rows: int, cols: int, grid: list) -> list:
|
||||||
"""奇数行(0,2,4...)左→右，偶数行(1,3,5...)右→左"""
|
"""生成点位级蛇形路径：遍历点位行 0→rows，奇数行左→右，偶数行右→左
|
||||||
|
|
||||||
|
每个点位 (pr, c) 同时服务：
|
||||||
|
- 正面：机器 (pr, c)（如果 pr < rows 且 grid[pr][c] 为真）
|
||||||
|
- 背面：机器 (pr-1, c)（如果 pr > 0 且 grid[pr-1][c] 为真）
|
||||||
|
|
||||||
|
按此路径走完所有点位，是最短的蛇形走位，不再反复横跳。
|
||||||
|
"""
|
||||||
path = []
|
path = []
|
||||||
for r in range(rows):
|
for pr in range(rows + 1): # 点位行 = 机器行 + 1
|
||||||
if r % 2 == 0:
|
if pr % 2 == 0:
|
||||||
for c in range(cols):
|
for c in range(cols):
|
||||||
if MissionExecutorV3._has_machine(grid, r, c):
|
path.append((pr, c))
|
||||||
path.append((r, c))
|
|
||||||
else:
|
else:
|
||||||
for c in range(cols - 1, -1, -1):
|
for c in range(cols - 1, -1, -1):
|
||||||
if MissionExecutorV3._has_machine(grid, r, c):
|
path.append((pr, c))
|
||||||
path.append((r, c))
|
|
||||||
return path
|
return path
|
||||||
|
|
||||||
@staticmethod
|
@staticmethod
|
||||||
@@ -461,6 +489,15 @@ class MissionExecutorV3:
|
|||||||
|
|
||||||
# ==================== 点位查找 ====================
|
# ==================== 点位查找 ====================
|
||||||
|
|
||||||
|
@staticmethod
|
||||||
|
def _find_any_position(positions: list, row: int, col: int) -> Optional[dict]:
|
||||||
|
"""查找点位的任意有效坐标（正面/背面坐标相同，取第一个有坐标的）"""
|
||||||
|
for side in ("front", "back"):
|
||||||
|
p = MissionExecutorV3._find_point(positions, row, col, side)
|
||||||
|
if p and MissionExecutorV3._has_coords(p):
|
||||||
|
return p
|
||||||
|
return None
|
||||||
|
|
||||||
@staticmethod
|
@staticmethod
|
||||||
def _find_point(positions: list, row: int, col: int, side: str) -> Optional[dict]:
|
def _find_point(positions: list, row: int, col: int, side: str) -> Optional[dict]:
|
||||||
for p in positions:
|
for p in positions:
|
||||||
@@ -484,6 +521,25 @@ class MissionExecutorV3:
|
|||||||
|
|
||||||
# ==================== 二维码扫描 ====================
|
# ==================== 二维码扫描 ====================
|
||||||
|
|
||||||
|
|
||||||
|
def _wait_arm_ready(self, target_angles: list, timeout: float = 10.0, tolerance: float = 3.0) -> bool:
|
||||||
|
"""等待机械臂稳定到目标角度（±tolerance 度），超时返回 False"""
|
||||||
|
if not self.arm_client:
|
||||||
|
return True
|
||||||
|
deadline = time.time() + timeout
|
||||||
|
while time.time() < deadline:
|
||||||
|
try:
|
||||||
|
ok, current_rad = self.arm_client.get_angles()
|
||||||
|
if ok and current_rad and len(current_rad) >= 6:
|
||||||
|
# get_angles() 返回弧度，target_angles 是角度，需转换后比较
|
||||||
|
current_deg = [math.degrees(a) for a in current_rad]
|
||||||
|
if all(abs(current_deg[i] - target_angles[i]) < tolerance for i in range(6)):
|
||||||
|
return True
|
||||||
|
except Exception:
|
||||||
|
pass
|
||||||
|
time.sleep(0.5)
|
||||||
|
self._log(f" ⚠️ 机械臂稳定等待超时 (target={target_angles})")
|
||||||
|
return False
|
||||||
def _scan_qr_with_poses(self, qr_configs: list) -> Optional[str]:
|
def _scan_qr_with_poses(self, qr_configs: list) -> Optional[str]:
|
||||||
"""用二维码配置中的姿态依次尝试，逐一调整姿态+等2秒+扫码，全部失败才弹框"""
|
"""用二维码配置中的姿态依次尝试，逐一调整姿态+等2秒+扫码，全部失败才弹框"""
|
||||||
if not qr_configs:
|
if not qr_configs:
|
||||||
@@ -502,7 +558,7 @@ class MissionExecutorV3:
|
|||||||
# 调整机械臂姿态
|
# 调整机械臂姿态
|
||||||
if self.arm_client:
|
if self.arm_client:
|
||||||
self.arm_client.set_angles(angles, speed=500)
|
self.arm_client.set_angles(angles, speed=500)
|
||||||
time.sleep(2) # 等机械臂稳定 + 摄像头图像稳定
|
self._wait_arm_ready(angles)
|
||||||
# 读取摄像头并扫码
|
# 读取摄像头并扫码
|
||||||
qr = self._decode_qr_from_arm()
|
qr = self._decode_qr_from_arm()
|
||||||
if qr:
|
if qr:
|
||||||
@@ -601,7 +657,7 @@ class MissionExecutorV3:
|
|||||||
# 调整机械臂
|
# 调整机械臂
|
||||||
if self.arm_client:
|
if self.arm_client:
|
||||||
self.arm_client.set_angles(angles, speed=500)
|
self.arm_client.set_angles(angles, speed=500)
|
||||||
time.sleep(QR_POSE_WAIT)
|
self._wait_arm_ready(angles)
|
||||||
|
|
||||||
# 拍照
|
# 拍照
|
||||||
path = self._capture_arm_photo(row, col, side, pi + 1, qr_value)
|
path = self._capture_arm_photo(row, col, side, pi + 1, qr_value)
|
||||||
|
|||||||
Reference in New Issue
Block a user