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