1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39 from vec2d import *
40 from lineline import *
41 from math import *
42
43 import geometry
44 from actions import *
45 import state
46 import rrtTree
47 import random
48
49
51 """
52 Implements a tactic in which an agent uses a PID controler
53 to reach its goal position.
54 """
55
56 - def __init__(self, agentIndex, goalPos, rulesOfTheSea):
57
58 self.shipIndex = agentIndex
59 self.goalPos = goalPos
60 self.rulesOfTheSea = rulesOfTheSea
61
62 self.anglePID = 0.5
63 self.distPID = 0.1
64
65 self.distIConst = 0.00001
66 self.distIntergral = 0
67
68
84
86 """
87 Composes a PID-controller-based action in order to reach the goal.
88 This function is by default used in unless there is a collision danger.
89 """
90 estimatedState = agentWorldModel.getEstimatedExtShipState()
91 estimatedPos = (estimatedState.x, estimatedState.y)
92
93 distanceToGoal = geometry.distance2D(estimatedPos, self.goalPos)
94 angleFixToGoal = self.getAngleFixToGoal(estimatedState, self.goalPos)
95
96
97
98 if distanceToGoal <= 10:
99 action = CompositeAction()
100 action.appendAction(PrimitiveAction("stop", []))
101 return action
102
103
104 self.distIntergral += distanceToGoal
105
106 action = CompositeAction()
107
108 if abs(angleFixToGoal) > 10:
109 requiredSteering = angleFixToGoal * self.anglePID
110
111
112 steering = max(min(requiredSteering, 90), -90)
113 action.appendAction(PrimitiveAction("setSteering", [steering]))
114
115
116
117 requiredSpeed = distanceToGoal * self.distPID + self.distIConst * self.distIntergral
118
119 speed = max(min(requiredSpeed, 40), -10)
120
121
122 action.appendAction(PrimitiveAction("setEngineSpeed", [speed]))
123
124 return action
125
126
128 """
129 According to the estimation, how much should we turn
130 to point at the goal
131
132 Keyword arguments:
133 estimatedState -- estimated ShipExternalState
134
135 returns:
136 The angle we should turn
137 """
138 estimatedPos = (estimatedState.x, estimatedState.y)
139 desiredOrientation = geometry.orientation2D(estimatedPos, goal)
140 estimatedOrientation = estimatedState.orientation
141 return geometry.computeShortestTurn(estimatedOrientation, desiredOrientation)
142
144 """
145 Compute a list of ships that are on path to potential collision
146 with the agent's ship.
147
148 @type agentWorldModel: AgentWorldModel
149 @param agentWorldModel: The belief state of the agent
150
151 @rtype: a list of ShipExternalState
152 @return: External states (position, orientation...) of ships that are on a path to potential collision with the agent's ship.
153 """
154
155
156 estimatedSelfState = agentWorldModel.getEstimatedExtShipState()
157 myPos = Vec2d(estimatedSelfState.x, estimatedSelfState.y)
158
159 myOrient = estimatedSelfState.orientation
160
161 mySpeed = estimatedSelfState.speed
162
163
164
165 myDirection = Vec2d(cos(radians(myOrient)), sin(radians(myOrient)))
166
167 myLineP1 = myPos - myDirection * 10000
168 myLineP2 = myPos + myDirection * 10000
169
170
171
172 dangerShipsStates = []
173 externalShipStates = agentWorldModel.getEstimatedStatesOfOtherShips(self.shipIndex)
174 for state in externalShipStates:
175
176 otherPos = Vec2d(state.x, state.y)
177
178 otherOrient = state.orientation
179
180 otherSpeed = state.speed
181
182
183
184 otherDirection = Vec2d(cos(radians(otherOrient)), sin(radians(otherOrient)))
185
186 otherLineP1 = otherPos - otherDirection * 10000
187 otherLineP2 = otherPos + otherDirection * 10000
188
189
190
191
192 globalAngleToOtherShip = (otherPos - myPos).get_angle()
193 relativeAngleToOtherShip = geometry.computeShortestTurn(myOrient, globalAngleToOtherShip)
194
195 if 45 > relativeAngleToOtherShip > -45:
196 relativeOrient = geometry.computeShortestTurn(myOrient, otherOrient)
197
198 if abs(relativeOrient) > 135:
199 distToOther = (otherPos - myPos).get_length()
200
201 sumSpeeds = mySpeed + otherSpeed
202
203 timeToCollision = distToOther / sumSpeeds if sumSpeeds > 0 else 999999
204
205
206 if timeToCollision < 60:
207 dangerShipsStates.append(state)
208 continue
209
210
211
212
213 intersection = lineline(myLineP1, myLineP2, otherLineP1, otherLineP2)
214
215
216 if intersection:
217 myDistToIntersect= (intersection - myPos).get_length()
218
219 otherDistToIntersect = (intersection - otherPos).get_length()
220
221
222
223 if myDistToIntersect > -50 and otherDistToIntersect > -50:
224 myTimeToIntersect = myDistToIntersect / mySpeed if mySpeed > 0 else 999999
225
226 otherTimeToIntersect = otherDistToIntersect / otherSpeed if otherSpeed > 0 else 999999
227
228
229
230 if myTimeToIntersect < 60 and otherTimeToIntersect < 60 and abs(myTimeToIntersect - otherTimeToIntersect) < 30:
231
232 dangerShipsStates.append(state)
233
234
235 return dangerShipsStates
236
237
238
239
241 """
242 Compute an action to take when there is a collision danger.
243
244 @type shipsToAvoid: a list of ShipExternalState
245 @param shipsToAvoid: External states (position, orientation...) of ships that are on a path to potential collision with the agent's ship.
246
247 @type agentWorldModel: AgentWorldModel
248 @param agentWorldModel: The belief state of the agent
249
250 @rtype: CompositeAction
251 @return: Action to take in order to avoid collision
252 """
253
254 estimatedSelfState = agentWorldModel.getEstimatedExtShipState()
255 myPos = Vec2d(estimatedSelfState.x, estimatedSelfState.y)
256 myOrient = estimatedSelfState.orientation
257
258 turnRight = False
259 stop = False
260 for state in shipsToAvoid:
261 otherShipPos = Vec2d(state.x, state.y)
262 otherShipOrientation = state.orientation
263 globalAngleToOtherShip = (otherShipPos - myPos).get_angle()
264 relativeAngleToOtherShip = geometry.computeShortestTurn(myOrient, globalAngleToOtherShip)
265 relativeOrient = geometry.computeShortestTurn(myOrient, otherShipOrientation)
266 if abs(relativeAngleToOtherShip) < 45:
267 if abs(relativeOrient) > 135:
268
269 turnRight = True
270 elif abs(relativeOrient) < 45:
271
272 stop = True
273 elif 45 < relativeAngleToOtherShip < 135:
274
275
276 stop = True
277 elif -135 < relativeAngleToOtherShip < -45:
278
279 turnRight = True
280 else:
281
282 pass
283
284
285 if stop:
286
287 action = CompositeAction()
288
289
290
291 action.appendAction(PrimitiveAction("setEngineSpeed", [-10]))
292 return action
293
294 if turnRight:
295
296 action = CompositeAction()
297
298
299
300 action.appendAction(PrimitiveAction("setSteering", [30]))
301 return action
302
303
304 return self.composePIDAction(agentWorldModel)
305
306
307
309 """
310 This class implements an RRT based partolling agent tactic.
311 Given a start position and a goal position, it runs a simplistic RRT
312 algorithm, and extracts the resulting path.
313 """
314 - def __init__(self, agentIndex, startPos, goalPos, obstacleList, rulesOfTheSea):
315 """
316 @type agentIndex: int
317 @param agentIndex: the index of the patroling agent (like an agent-id)
318
319 @type startPos: 2-tuple
320 @param startPos: start position for the RRT computation
321
322 @type goalPos: 2-tuple
323 @param goalPos: goal position for the RRT computation
324
325 @type rulesOfTheSea: boolean
326 @param rulesOfTheSea: Tells whether to respect the rules of the sea
327 """
328
329 self.agentIndex = agentIndex
330 self.startPos = state.ShipExternalState(startPos[0], startPos[1], 0, 0, 0)
331 self.goalPos = state.ShipExternalState(goalPos[0], goalPos[1], 0, 0, 0)
332 self.obstacleList = obstacleList
333 self.rulesOfTheSea = rulesOfTheSea
334 self.actionIndex = 0
335
336
337
338
339
340
341
342 self.angleThreshold = 10
343 self.distanceThreshold = 10
344
345
346 xStart, yStart = startPos
347 xEnd, yEnd = goalPos
348
349 self.minX = int(round(min(xStart, xEnd) - 200))
350 self.maxX = int(round(max(xStart, xEnd) + 200))
351 self.minY = int(round(min(yStart, yEnd) - 200))
352 self.maxY = int(round(max(yStart, yEnd) + 200))
353 self.minAng = 0
354 self.maxAng = 360
355
356
357 self.epsilon = 0.1
358
359
360
361
362
363
364 self.tree = rrtTree.RRTTreeXYOrientationEuclidDist(self.startPos, distanceUnit=10, angleUnit=5)
365 self.path = self.rrtPath(self.startPos, self.goalPos)
366
367
369 """
370 Returns the path of points along which the agent patrols.
371
372 @rtype: list of 2-tuples
373 @return: a list of (x, y) points representing the patrol path
374 """
375 return self.path
376
377 - def rrtPath(self, startPos, goalPos):
378 """
379 Computes an RRT path from startPos to goalPos.
380
381 @type startPos: 2-tuple
382 @param startPos: start position for the RRT computation
383
384 @type goalPos: 2-tuple
385 @param goalPos: goal position for the RRT computation
386
387 @rtype: list of 2-tuples
388 @return: a list of (x, y) points representing the patrol path
389 """
390
391 initialExternalState = startPos
392
393
394
395 currentState = initialExternalState
396 while not self.isGoalState(currentState):
397 currentState = self.extend(self.tree, self.chooseRandomState())
398
399 nodesPath = self.tree.getNodesPathTo(currentState)
400 path = [(n.state.x, n.state.y) for n in nodesPath]
401 return path
402
403
404 - def extend(self, tree, randomState):
405 """
406 The EXTEND function from the paper.
407 Here, in this simple version of the algorithms, we just
408 take an epsilon step towards the randomState, without checking
409 the all intermediate states are collision-free.
410 We also assume holonomic problem in which the computation
411 of the edge (the action that takes us to the new state)
412 is just a simple vector calculation.
413
414
415 @type randomState: ShipExternalState
416 @param randomState: A state that was randomly sampled in space, towards which we take an epsilon step.
417
418 @rtype: ShipExternalState
419 @return: The new state that was added to the tree.
420 """
421 closestNode = tree.findClosestNode(randomState)
422 closestState = closestNode.state
423
424
425 for obst in self.obstacleList:
426 A = Vec2d(closestState.x, closestState.y)
427 B = Vec2d(randomState.x, randomState.y)
428 C, D = obst.getBorder()
429 C = Vec2d(C)
430 D = Vec2d(D)
431
432 if lineline(A, B, C, D):
433
434 return None
435
436
437
438
439 closestStateX = closestState.x
440 closestStateY = closestState.y
441 closestStateOrient = closestState.orientation
442 newX = (randomState.x - closestStateX) * self.epsilon + closestStateX
443 newY = (randomState.y - closestStateY) * self.epsilon + closestStateY
444 newOrient = (randomState.orientation - closestStateOrient) * self.epsilon + closestStateOrient
445
446
447
448 xDiff = newX - closestStateX
449 yDiff = newY - closestStateY
450
451 if closestNode.parent != None:
452 oldOrientation = degrees( atan2(closestStateY - closestNode.parent.state.y,
453 closestStateX - closestNode.parent.state.x) )
454 else:
455 oldOrientation = closestStateOrient
456
457 steeringToApply = degrees( atan2(yDiff, xDiff) ) - oldOrientation
458
459 speedToApply = sqrt( xDiff * xDiff + yDiff * yDiff )
460 action = CompositeAction()
461 action.appendAction(PrimitiveAction("setEngineSpeed", [speedToApply]))
462 action.appendAction(PrimitiveAction("setSteering", [steeringToApply]))
463
464
465 newState = state.ShipExternalState(newX, newY, newOrient, speedToApply, 0)
466 parent = closestNode
467 edge = action
468 newNode = rrtTree.RRTNode(newState, parent, edge)
469 tree.addNode(newNode)
470
471 return newState
472
474 "Is the ship within some threshold from the goal."
475
476 if not shipState:
477 return False
478
479 point1 = (shipState.x, shipState.y)
480 point2 = (self.goalPos.x, self.goalPos.y)
481 distance = geometry.distance2D(point1, point2)
482 if distance > self.distanceThreshold:
483 return False
484
485
486
487
488
489
490
491 return True
492
493
495 "Samples a random state with a bias towards the goal."
496 if random.random() < 0.05:
497 return self.goalPos
498 else:
499 return state.ShipExternalState(random.randint(self.minX, self.maxX),
500 random.randint(self.minY, self.maxY),
501 random.randint(self.minAng, self.maxAng),
502 speed = -1, angularSpeed = -1)
503
504
505
506
507
508
509
510
511
512
513
514
516 """
517 This is a base class for low level code that navigates to specific point.
518 """
520 "Given the (inferred) state of the world, decide on a set of actions."
521 abstract
522
523 - def done(self, agentWorldModel):
524 "Is goal reached."
525 abstract
526
527 - def atPoint(self, estimatedPos, point):
528 distanceToGoal = geometry.distance2D(estimatedPos, point)
529
530 return (distanceToGoal <= 50)
531
533 """
534 Navigation assumed to have a goal position.
535 Thie method returns this goal.
536 """
537 abstract
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
555 """
556 Low level tactic for PID based navigation to a point
557 """
558 - def __init__(self, agentIndex, goalPoint, rulesOfTheSea):
559
560
561 self.goalPoint = goalPoint
562 self.worker = AgentPIDTacticImpl(agentIndex, goalPoint, rulesOfTheSea)
563
565 "Implements abstract method."
566 return self.worker.composeAction(agentWorldModel)
567
568 - def done(self, agentWorldModel):
569 "Implements abstract method."
570 estimatedState = agentWorldModel.getEstimatedExtShipState()
571 estimatedPos = (estimatedState.x, estimatedState.y)
572 return self.atPoint(estimatedPos, self.goalPoint)
573
575 "Implements abstract method."
576 return self.goalPoint
577
579 """
580 Low level tactic for RRT based navigation to a point
581 """
582 - def __init__(self, agentIndex,
583 estimatedPos,
584 goalPoint,
585 estimatedObstaclesList,
586 rulesOfTheSea):
587
588 self.agentIndex = agentIndex
589 self.rrtBuilder = AgentRRTTacticImpl(agentIndex,
590 estimatedPos,
591 goalPoint,
592 estimatedObstaclesList,
593 rulesOfTheSea)
594 self.rrtPath = self.rrtBuilder.path
595
596
597
598 self.goalPoint = self.rrtPath[-1]
599 self.nextRRTPointIndex = 0
600 nextPoint = self.rrtPath[self.nextRRTPointIndex]
601 self.pidTactic = AgentPIDTactic(agentIndex, nextPoint, rulesOfTheSea)
602 self.rulesOfTheSea = rulesOfTheSea
603
605 "Implements abstract method."
606 estimatedState = agentWorldModel.getEstimatedExtShipState()
607 estimatedPos = (estimatedState.x, estimatedState.y)
608 nextPoint = self.rrtPath[self.nextRRTPointIndex]
609 if self.atPoint(estimatedPos, nextPoint):
610
611 self.nextRRTPointIndex += 1
612 nextPoint = self.rrtPath[self.nextRRTPointIndex]
613 self.pidTactic = AgentPIDTactic(self.agentIndex,
614 nextPoint,
615 self.rulesOfTheSea)
616 return self.pidTactic.composeAction(agentWorldModel)
617
618 - def done(self, agentWorldModel):
619 "Implements abstract method."
620 estimatedState = agentWorldModel.getEstimatedExtShipState()
621 estimatedPos = (estimatedState.x, estimatedState.y)
622 return self.atPoint(estimatedPos, self.goalPoint)
623
625 "Implements abstract method."
626 return self.goalPoint
627