Module obstacleAvoidance
[hide private]
[frames] | no frames]

Source Code for Module obstacleAvoidance

  1  ################################################################################## 
  2  # Copyright (c) 2010, 2011, 2012, 2013, Daniel Urieli, Peter Stone 
  3  # University of Texas at Austin 
  4  # All right reserved 
  5  #  
  6  # Based On: 
  7  #  
  8  # Copyright (c) 2000-2003, Jelle Kok, University of Amsterdam 
  9  # All rights reserved. 
 10  #  
 11  # Redistribution and use in source and binary forms, with or without 
 12  # modification, are permitted provided that the following conditions are met: 
 13  #  
 14  # 1. Redistributions of source code must retain the above copyright notice, this 
 15  # list of conditions and the following disclaimer. 
 16  #  
 17  # 2. Redistributions in binary form must reproduce the above copyright notice, 
 18  # this list of conditions and the following disclaimer in the documentation 
 19  # and/or other materials provided with the distribution. 
 20  #  
 21  # 3. Neither the name of the University of Amsterdam nor the names of its 
 22  # contributors may be used to endorse or promote products derived from this 
 23  # software without specific prior written permission. 
 24  #  
 25  # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
 26  # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 
 27  # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 
 28  # DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE 
 29  # FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 
 30  # DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 
 31  # SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 
 32  # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 
 33  # OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 
 34  # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 
 35  ################################################################################## 
 36   
 37   
 38   
 39  #polarsToPotentialObstacles() 
 40  #haveFreeCorridor() 
 41  #computeCorridor() 
 42  #polarsToObstaclesInsideCorridor() 
 43  from vec2d import * 
 44  from lineline import * 
 45   
46 -def pathBlocked(curPos, nextPos, obstacleList):
47 for obst in obstacleList: 48 A = Vec2d(curPos) 49 B = Vec2d(nextPos) 50 C, D = obst.getBorder() 51 C = Vec2d(C) 52 D = Vec2d(D) 53 54 if lineline(A, B, C, D): 55 #intersects! 56 return True 57 return False
58 59 60 61 # # We decided not to use the below (incompleted) function for now, as we already have the RRT 62 63 # def avoidObstacles(estimatedState, goalPos, estimatedObstaclesList): 64 # """ 65 # Given a distance and angle to the desired goal, and a list 66 # of estimated obstacles, return a new distance and angle 67 # to a temporary target on a path that avoids the current target. 68 # 69 # @type estimated: ShipExternalState 70 # @param estimated: estimated state of the ship 71 # 72 # @type goalPos: (x, y) tuple of doubles 73 # @param goalPos: goal position of the ship 74 # 75 # @rtype : a pair of doubles 76 # @return : a new goal (x, y) 77 # """ 78 # # Implement the following decision scheme 79 # # If we are about to collide: if possible back up, otherwise stop. 80 # # Else, do we have a "corridor" to target? 81 # # ->Yes: GO 82 # # ->No: can deviation help? (i.e. is corridor narrowed from one side only?) 83 # # ->Yes: deviate 84 # # ->No: there is a block - look right and left and take the smallest deviation that would bring you to the edge 85 # 86 # # TODO: this should be a parameter? 87 # SAFETY_RADIUS = 5 # ship doesn't get closer than that to obstacles 88 # 89 # # if about to collide stop 90 # # TODO: implement 91 # 92 # # do we have a corridor? 93 # polarsToCheck = polarsToPotentialObstacles(estimatedState, 94 # goalPos, 95 # estimatedObstaclesList) 96 # corridor = computeCorridor(estimatedState, goalPos, SAFETY_RADIUS) 97 # polarsInsideCorridor = polarsToObstaclesInsideCorridor(polarsToCheck, 98 # corridor) 99 # if haveFreeCorridor(polarsInsideCorridor): 100 # # no avoidance action is needed 101 # return goalPos 102 # 103 # # else blocked, check in what way 104