|
2 | 2 | from enum import Enum
|
3 | 3 | from typing import List, Optional
|
4 | 4 |
|
5 |
| -from alitra import Frame, Orientation, Pose, Position |
6 | 5 | from pydantic import BaseModel, Field
|
7 | 6 |
|
8 | 7 | from isar.apis.models.models import InputPose, InputPosition
|
@@ -44,188 +43,139 @@ class StartMissionInspectionDefinition(BaseModel):
|
44 | 43 | analysis_type: Optional[str] = None
|
45 | 44 | duration: Optional[float] = None
|
46 | 45 | metadata: Optional[dict] = None
|
47 |
| - id: Optional[str] = None |
48 | 46 |
|
49 | 47 |
|
50 | 48 | class StartMissionTaskDefinition(BaseModel):
|
51 | 49 | type: TaskType = Field(default=TaskType.Inspection)
|
52 | 50 | pose: InputPose
|
53 | 51 | inspection: Optional[StartMissionInspectionDefinition] = None
|
54 | 52 | tag: Optional[str] = None
|
55 |
| - id: Optional[str] = None |
56 | 53 | zoom: Optional[ZoomDescription] = None
|
57 | 54 |
|
58 | 55 |
|
59 | 56 | class StartMissionDefinition(BaseModel):
|
60 | 57 | tasks: List[StartMissionTaskDefinition]
|
61 |
| - id: Optional[str] = None |
62 | 58 | name: Optional[str] = None
|
63 | 59 | start_pose: Optional[InputPose] = None
|
64 |
| - dock: Optional[bool] = None |
65 |
| - undock: Optional[bool] = None |
| 60 | + dock: Optional[bool] = Field(default=False) |
| 61 | + undock: Optional[bool] = Field(default=False) |
66 | 62 |
|
67 | 63 |
|
68 |
| -def to_isar_mission(start_mission_definition: StartMissionDefinition) -> Mission: |
| 64 | +def to_isar_mission( |
| 65 | + start_mission_definition: StartMissionDefinition, |
| 66 | + return_pose: Optional[InputPose] = None, |
| 67 | +) -> Mission: |
69 | 68 | isar_tasks: List[TASKS] = []
|
70 | 69 |
|
71 |
| - for start_mission_task_definition in start_mission_definition.tasks: |
72 |
| - task: TASKS = create_isar_task(start_mission_task_definition) |
73 |
| - if start_mission_task_definition.id: |
74 |
| - task.id = start_mission_task_definition.id |
| 70 | + for task_definition in start_mission_definition.tasks: |
| 71 | + task: TASKS = to_isar_task(task_definition) |
75 | 72 | isar_tasks.append(task)
|
76 | 73 |
|
| 74 | + if return_pose: |
| 75 | + isar_tasks.append(ReturnToHome(pose=return_pose.to_alitra_pose())) |
| 76 | + |
77 | 77 | if not isar_tasks:
|
78 | 78 | raise MissionPlannerError("Mission does not contain any valid tasks")
|
79 | 79 |
|
80 |
| - check_for_duplicate_ids(isar_tasks) |
81 |
| - |
82 |
| - isar_mission: Mission = Mission(tasks=isar_tasks) |
83 |
| - |
84 |
| - isar_mission.dock = start_mission_definition.dock |
85 |
| - isar_mission.undock = start_mission_definition.undock |
86 |
| - |
| 80 | + isar_mission_name: str |
87 | 81 | if start_mission_definition.name:
|
88 |
| - isar_mission.name = start_mission_definition.name |
| 82 | + isar_mission_name = start_mission_definition.name |
89 | 83 | else:
|
90 |
| - isar_mission.name = _build_mission_name() |
91 |
| - |
92 |
| - if start_mission_definition.id: |
93 |
| - isar_mission.id = start_mission_definition.id |
| 84 | + isar_mission_name = _build_mission_name() |
94 | 85 |
|
| 86 | + start_pose = None |
95 | 87 | if start_mission_definition.start_pose:
|
96 |
| - input_pose: InputPose = start_mission_definition.start_pose |
97 |
| - input_frame: Frame = Frame(name=input_pose.frame_name) |
98 |
| - input_position: Position = Position( |
99 |
| - input_pose.position.x, |
100 |
| - input_pose.position.y, |
101 |
| - input_pose.position.z, |
102 |
| - input_frame, |
103 |
| - ) |
104 |
| - input_orientation: Orientation = Orientation( |
105 |
| - input_pose.orientation.x, |
106 |
| - input_pose.orientation.y, |
107 |
| - input_pose.orientation.z, |
108 |
| - input_pose.orientation.w, |
109 |
| - input_frame, |
110 |
| - ) |
111 |
| - isar_mission.start_pose = Pose( |
112 |
| - position=input_position, orientation=input_orientation, frame=input_frame |
113 |
| - ) |
114 |
| - |
115 |
| - return isar_mission |
116 |
| - |
117 |
| - |
118 |
| -def check_for_duplicate_ids(items: List[TASKS]): |
119 |
| - duplicate_ids = get_duplicate_ids(items=items) |
120 |
| - if len(duplicate_ids) > 0: |
121 |
| - raise MissionPlannerError( |
122 |
| - f"Failed to create as there were duplicate IDs which is not allowed " |
123 |
| - f"({duplicate_ids})" |
124 |
| - ) |
125 |
| - |
| 88 | + start_pose = start_mission_definition.start_pose.to_alitra_pose() |
| 89 | + |
| 90 | + return Mission( |
| 91 | + tasks=isar_tasks, |
| 92 | + name=isar_mission_name, |
| 93 | + start_pose=start_pose, |
| 94 | + dock=start_mission_definition.dock, |
| 95 | + undock=start_mission_definition.undock, |
| 96 | + ) |
126 | 97 |
|
127 |
| -def create_isar_task(start_mission_task_definition) -> TASKS: |
128 | 98 |
|
129 |
| - if start_mission_task_definition.type == TaskType.Inspection: |
130 |
| - return create_inspection_task(start_mission_task_definition) |
131 |
| - elif start_mission_task_definition.type == TaskType.Localization: |
132 |
| - return create_localization_task(start_mission_task_definition) |
133 |
| - elif start_mission_task_definition.type == TaskType.ReturnToHome: |
134 |
| - return create_return_to_home_task(start_mission_task_definition) |
135 |
| - elif start_mission_task_definition.type == TaskType.Dock: |
| 99 | +def to_isar_task(task_definition: StartMissionTaskDefinition) -> TASKS: |
| 100 | + if task_definition.type == TaskType.Inspection: |
| 101 | + return to_inspection_task(task_definition) |
| 102 | + elif task_definition.type == TaskType.Localization: |
| 103 | + return to_localization_task(task_definition) |
| 104 | + elif task_definition.type == TaskType.ReturnToHome: |
| 105 | + return create_return_to_home_task(task_definition) |
| 106 | + elif task_definition.type == TaskType.Dock: |
136 | 107 | return create_dock_task()
|
137 | 108 | else:
|
138 | 109 | raise MissionPlannerError(
|
139 |
| - f"Failed to create task: '{start_mission_task_definition.type}' is not a valid" |
| 110 | + f"Failed to create task: '{task_definition.type}' is not a valid" |
140 | 111 | )
|
141 | 112 |
|
142 | 113 |
|
143 |
| -def create_inspection_task( |
144 |
| - start_mission_task_definition: StartMissionTaskDefinition, |
145 |
| -) -> TASKS: |
| 114 | +def to_inspection_task(task_definition: StartMissionTaskDefinition) -> TASKS: |
| 115 | + inspection_definition = task_definition.inspection |
146 | 116 |
|
147 |
| - if start_mission_task_definition.inspection.type == InspectionTypes.image: |
| 117 | + if inspection_definition.type == InspectionTypes.image: |
148 | 118 | return TakeImage(
|
149 |
| - target=start_mission_task_definition.inspection.inspection_target.to_alitra_position(), |
150 |
| - tag_id=start_mission_task_definition.tag, |
151 |
| - robot_pose=start_mission_task_definition.pose.to_alitra_pose(), |
152 |
| - metadata=start_mission_task_definition.inspection.metadata, |
153 |
| - zoom=start_mission_task_definition.zoom, |
| 119 | + robot_pose=task_definition.pose.to_alitra_pose(), |
| 120 | + tag_id=task_definition.tag, |
| 121 | + target=task_definition.inspection.inspection_target.to_alitra_position(), |
| 122 | + metadata=task_definition.inspection.metadata, |
| 123 | + zoom=task_definition.zoom, |
154 | 124 | )
|
155 |
| - elif start_mission_task_definition.inspection.type == InspectionTypes.video: |
| 125 | + elif inspection_definition.type == InspectionTypes.video: |
156 | 126 | return TakeVideo(
|
157 |
| - target=start_mission_task_definition.inspection.inspection_target.to_alitra_position(), |
158 |
| - duration=start_mission_task_definition.inspection.duration, |
159 |
| - tag_id=start_mission_task_definition.tag, |
160 |
| - robot_pose=start_mission_task_definition.pose.to_alitra_pose(), |
161 |
| - metadata=start_mission_task_definition.inspection.metadata, |
162 |
| - zoom=start_mission_task_definition.zoom, |
| 127 | + robot_pose=task_definition.pose.to_alitra_pose(), |
| 128 | + tag_id=task_definition.tag, |
| 129 | + target=task_definition.inspection.inspection_target.to_alitra_position(), |
| 130 | + duration=inspection_definition.duration, |
| 131 | + metadata=task_definition.inspection.metadata, |
| 132 | + zoom=task_definition.zoom, |
163 | 133 | )
|
164 |
| - |
165 |
| - elif start_mission_task_definition.inspection.type == InspectionTypes.thermal_image: |
| 134 | + elif inspection_definition.type == InspectionTypes.thermal_image: |
166 | 135 | return TakeThermalImage(
|
167 |
| - target=start_mission_task_definition.inspection.inspection_target.to_alitra_position(), |
168 |
| - tag_id=start_mission_task_definition.tag, |
169 |
| - robot_pose=start_mission_task_definition.pose.to_alitra_pose(), |
170 |
| - metadata=start_mission_task_definition.inspection.metadata, |
171 |
| - zoom=start_mission_task_definition.zoom, |
| 136 | + robot_pose=task_definition.pose.to_alitra_pose(), |
| 137 | + tag_id=task_definition.tag, |
| 138 | + target=task_definition.inspection.inspection_target.to_alitra_position(), |
| 139 | + metadata=task_definition.inspection.metadata, |
| 140 | + zoom=task_definition.zoom, |
172 | 141 | )
|
173 |
| - |
174 |
| - elif start_mission_task_definition.inspection.type == InspectionTypes.thermal_video: |
| 142 | + elif inspection_definition.type == InspectionTypes.thermal_video: |
175 | 143 | return TakeThermalVideo(
|
176 |
| - target=start_mission_task_definition.inspection.inspection_target.to_alitra_position(), |
177 |
| - duration=start_mission_task_definition.inspection.duration, |
178 |
| - tag_id=start_mission_task_definition.tag, |
179 |
| - robot_pose=start_mission_task_definition.pose.to_alitra_pose(), |
180 |
| - metadata=start_mission_task_definition.inspection.metadata, |
181 |
| - zoom=start_mission_task_definition.zoom, |
| 144 | + robot_pose=task_definition.pose.to_alitra_pose(), |
| 145 | + tag_id=task_definition.tag, |
| 146 | + target=task_definition.inspection.inspection_target.to_alitra_position(), |
| 147 | + duration=inspection_definition.duration, |
| 148 | + metadata=task_definition.inspection.metadata, |
| 149 | + zoom=task_definition.zoom, |
182 | 150 | )
|
183 |
| - |
184 |
| - elif start_mission_task_definition.inspection.type == InspectionTypes.audio: |
| 151 | + elif inspection_definition.type == InspectionTypes.audio: |
185 | 152 | return RecordAudio(
|
186 |
| - target=start_mission_task_definition.inspection.inspection_target.to_alitra_position(), |
187 |
| - duration=start_mission_task_definition.inspection.duration, |
188 |
| - tag_id=start_mission_task_definition.tag, |
189 |
| - robot_pose=start_mission_task_definition.pose.to_alitra_pose(), |
190 |
| - metadata=start_mission_task_definition.inspection.metadata, |
191 |
| - zoom=start_mission_task_definition.zoom, |
| 153 | + robot_pose=task_definition.pose.to_alitra_pose(), |
| 154 | + tag_id=task_definition.tag, |
| 155 | + target=task_definition.inspection.inspection_target.to_alitra_position(), |
| 156 | + duration=inspection_definition.duration, |
| 157 | + metadata=task_definition.inspection.metadata, |
| 158 | + zoom=task_definition.zoom, |
192 | 159 | )
|
193 | 160 | else:
|
194 | 161 | raise ValueError(
|
195 |
| - f"Inspection type '{start_mission_task_definition.inspection.type}' not supported" |
| 162 | + f"Inspection type '{inspection_definition.type}' not supported" |
196 | 163 | )
|
197 | 164 |
|
198 | 165 |
|
199 |
| -def create_localization_task( |
200 |
| - start_mission_task_definition: StartMissionTaskDefinition, |
201 |
| -) -> Localize: |
202 |
| - return Localize( |
203 |
| - localization_pose=start_mission_task_definition.pose.to_alitra_pose() |
204 |
| - ) |
| 166 | +def to_localization_task(task_definition: StartMissionTaskDefinition) -> Localize: |
| 167 | + return Localize(localization_pose=task_definition.pose.to_alitra_pose()) |
205 | 168 |
|
206 | 169 |
|
207 | 170 | def create_return_to_home_task(
|
208 |
| - start_mission_task_definition: StartMissionTaskDefinition, |
| 171 | + task_definition: StartMissionTaskDefinition, |
209 | 172 | ) -> ReturnToHome:
|
210 |
| - return ReturnToHome(pose=start_mission_task_definition.pose.to_alitra_pose()) |
| 173 | + return ReturnToHome(pose=task_definition.pose.to_alitra_pose()) |
211 | 174 |
|
212 | 175 |
|
213 | 176 | def create_dock_task() -> DockingProcedure:
|
214 | 177 | return DockingProcedure(behavior="dock")
|
215 | 178 |
|
216 | 179 |
|
217 |
| -def get_duplicate_ids(items: List[TASKS]) -> List[str]: |
218 |
| - unique_ids: List[str] = [] |
219 |
| - duplicate_ids: List[str] = [] |
220 |
| - for item in items: |
221 |
| - id: str = item.id |
222 |
| - if id not in unique_ids: |
223 |
| - unique_ids.append(id) |
224 |
| - else: |
225 |
| - duplicate_ids.append(id) |
226 |
| - |
227 |
| - return duplicate_ids |
228 |
| - |
229 |
| - |
230 | 180 | def _build_mission_name() -> str:
|
231 | 181 | return f"{settings.PLANT_SHORT_NAME}{settings.ROBOT_NAME}{int(time.time())}"
|
0 commit comments