def __init__(self):
self.robotPos = Position(0, 0)
self.goalPos = None
self.occupancy_grid = OccupancyGrid()
self.occupancy_grid.initializeData()
self.subscribeEvents()
self.setMarkerPublishers()
self.markerUniqueId = 10
self.pathMarkerIds = []
self.openListMarkerIds = {}
def generateOccupancyGrid(self, cellSize = 0.1):
grid = OccupancyGrid(self._xll, self._yll, self._width, self._height, cellSize)
for l in self._lines:
# Note: element check is done by reference.
# This is valid, since each line is constructed only once.
if l in self._dynObstacles:
continue
x0 = l.getP1().getX()
y0 = l.getP1().getY()
x1 = l.getP2().getX()
y1 = l.getP2().getY()
grid.addLine(x0, y0, x1, y1)
return grid
def getOccupancyGrid(self, cellSize = 0.1):
if self._grid is None:
self._grid = OccupancyGrid(self._xll, self._yll, self._width, self._height, cellSize)
for l in self._lines:
x0 = l.getP1().getX()
y0 = l.getP1().getY()
x1 = l.getP2().getX()
y1 = l.getP2().getY()
self._grid.addLine(x0, y0, x1, y1)
return self._grid
def __init__(self):
self.robotPos = Position(0,0)
self.goalPos = None
self.occupancy_grid = OccupancyGrid()
self.occupancy_grid.initializeData()
self.subscribeEvents()
self.setMarkerPublishers()
self.markerUniqueId = 10
self.pathMarkerIds = []
self.openListMarkerIds = {}
class AStarPlanning:
def __init__(self):
self.robotPos = Position(0, 0)
self.goalPos = None
self.occupancy_grid = OccupancyGrid()
self.occupancy_grid.initializeData()
self.subscribeEvents()
self.setMarkerPublishers()
self.markerUniqueId = 10
self.pathMarkerIds = []
self.openListMarkerIds = {}
def getMarkerUniqueId(self):
self.markerUniqueId += 1
return self.markerUniqueId
def setMarkerPublishers(self):
self.markerPublisher = rospy.Publisher('/planning_markers', Marker)
self.markerArray_publisher = rospy.Publisher(
'/planning_multiple_markers', MarkerArray, latch=True)
def subscribeEvents(self):
rospy.Subscriber('/goal_pos', PoseStamped, self.rvizGoalPosCallBack)
def rvizGoalPosCallBack(self, clickedPose):
comandType = rospy.get_param('comandType')
pos_x = int(clickedPose.pose.position.x)
pos_y = int(clickedPose.pose.position.y)
if (comandType == "o"):
clickedPosition = Position(pos_x, pos_y)
self.occupancy_grid.setData(clickedPosition.x, clickedPosition.y,
100)
elif (comandType == "g"):
r = rospy.Rate(4)
self.publisRobotPoseMarker()
self.deletePathMarkers()
r.sleep()
self.updatePlanning = True
self.goalPos = Position(pos_x, pos_y)
def deletePathMarkers(self):
for i in range(len(self.pathMarkerIds)):
pathMarkerId = self.pathMarkerIds[i]
marker = Marker()
marker.header.frame_id = '/map'
marker.ns = 'AStartPlanning'
marker.id = pathMarkerId
marker.action = Marker.DELETE
self.markerPublisher.publish(marker)
def publisText(self, s):
marker = Marker()
shape = Marker.TEXT_VIEW_FACING
marker.header.frame_id = '/map'
marker.header.stamp = rospy.Time() #rospy.Time.now()
marker.ns = 'AStartPlanning'
marker.id = 5
marker.type = shape
marker.action = Marker.ADD
marker.text = s
marker.pose.position.x = 17.0
marker.pose.position.y = -3.0
marker.pose.position.z = 2.0
marker.scale.z = 1.1
marker.color.r = 0.8
marker.color.g = 0.0
marker.color.b = 0.0
marker.color.a = 0.9
self.markerPublisher.publish(marker)
def publisDrawPathLine(self):
self.path.reverse()
r = rospy.Rate(5)
for i in range(len(self.path) - 1):
pathPoint1 = self.path[i]
pathPoint2 = self.path[i + 1]
self.robotPos = Position(int(pathPoint2.x), int(pathPoint2.y))
marker = Marker()
shape = Marker.LINE_STRIP
marker.header.frame_id = '/map'
marker.header.stamp = rospy.Time() #rospy.Time.now()
marker.ns = 'AStartPlanning'
marker.id = self.getMarkerUniqueId()
self.pathMarkerIds.append(marker.id)
marker.type = shape
marker.action = Marker.ADD
marker.points = [pathPoint1, pathPoint2]
marker.scale.x = 0.4
marker.scale.y = 1.0
marker.scale.z = 1.0
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 1.0
marker.color.a = 1.0
self.publisRobotPoseMarker()
self.markerPublisher.publish(marker)
r.sleep()
def robotPosChangedCallback(self, robotPos):
self.robotPos = robotPos
self.cmd_robotPos_sub.unregister()
def goalPosChangedCallback(self, goalPos):
self.goalPos = goalPos
def computeEquledianDistance(self, currentCell):
xDistance = self.goalPos.x - currentCell.x
yDistance = self.goalPos.y - currentCell.y
return math.sqrt(math.pow(xDistance, 2) + math.pow(yDistance, 2))
def computeH(self, childGridCell):
childGridCell.hVal = self.computeEquledianDistance(childGridCell)
def oneCellMoveCost(self):
return 1
def isAnObstacle(self, gridCell):
gridCellData = self.occupancy_grid.data[gridCell.x][gridCell.y]
if (gridCellData == 100):
return True
return False
def getCellCost(self, gridCell):
if (self.isAnObstacle(gridCell)):
return 100
return 1
def computeG(self, gridCell):
if (gridCell.parentGridCell is None):
gridCell.gVal = 0
else:
gridCell.gVal = gridCell.parentGridCell.gVal + self.getCellCost(
gridCell)
def computeTotalCost(self, childGridCell):
self.computeG(childGridCell)
self.computeH(childGridCell)
return childGridCell.getCost()
def isGoalGridCell(self, currentCell):
return currentCell.x == self.goalPos.x and currentCell.y == self.goalPos.y
def isInRange(self, x, y):
return (x >= 0
and y >= 0) and (x < self.occupancy_grid.dimension.width
and y < self.occupancy_grid.dimension.height)
def getAdjacentCells(self, currentCell):
adjacentCells = []
x = currentCell.x
y = currentCell.y
if (self.isInRange(x + 1, y)):
gridCell = GridCell(x + 1, y, currentCell)
if (self.isAnObstacle(gridCell) == False):
adjacentCells.append(gridCell)
if (self.isInRange(x + 1, y + 1)):
gridCell = GridCell(x + 1, y + 1, currentCell)
if (self.isAnObstacle(gridCell) == False):
adjacentCells.append(gridCell)
if (self.isInRange(x, y + 1)):
gridCell = GridCell(x, y + 1, currentCell)
if (self.isAnObstacle(gridCell) == False):
adjacentCells.append(gridCell)
if (self.isInRange(x - 1, y + 1)):
gridCell = GridCell(x - 1, y + 1, currentCell)
if (self.isAnObstacle(gridCell) == False):
adjacentCells.append(gridCell)
if (self.isInRange(x - 1, y)):
gridCell = GridCell(x - 1, y, currentCell)
if (self.isAnObstacle(gridCell) == False):
adjacentCells.append(gridCell)
if (self.isInRange(x - 1, y - 1)):
gridCell = GridCell(x - 1, y - 1, currentCell)
if (self.isAnObstacle(gridCell) == False):
adjacentCells.append(gridCell)
if (self.isInRange(x, y - 1)):
gridCell = GridCell(x, y - 1, currentCell)
if (self.isAnObstacle(gridCell) == False):
adjacentCells.append(gridCell)
if (self.isInRange(x + 1, y - 1)):
gridCell = GridCell(x + 1, y - 1, currentCell)
if (self.isAnObstacle(gridCell) == False):
adjacentCells.append(gridCell)
return adjacentCells
def printAdjacentCells(self, adjs):
s = "Adjs : "
for i in range(len(adjs)):
adj = adjs[i]
s += "(" + str(adj.x) + "," + str(adj.y) + ") "
print s
def getPath(self, gridCell):
self.path.append(Point(gridCell.x + 0.5, gridCell.y + 0.5, 1))
if (gridCell.x == self.robotPos.x and gridCell.y == self.robotPos.y):
return
self.getPath(gridCell.parentGridCell)
def publisMultipleMarkers(self, dictionary):
l = dictionary.items()
marker_array = MarkerArray()
r = rospy.Rate(25)
life = 6
for i in range(len(l)):
(key, gridCell) = l[i]
marker = Marker()
marker.header.frame_id = '/map'
marker.header.stamp = rospy.Time()
marker.ns = 'AStartPlanning'
markerId = str(gridCell.x) + "_" + str(gridCell.y)
if (markerId in self.openListMarkerIds):
marker.id = self.openListMarkerIds[markerId]
else:
marker.id = self.getMarkerUniqueId()
self.openListMarkerIds[markerId] = marker.id
marker.type = Marker.CUBE
marker.action = Marker.ADD
marker.lifetime = rospy.Duration(life, 0)
marker.pose.position.x = gridCell.x + 0.5
marker.pose.position.y = gridCell.y + 0.5
marker.scale.x = 1.0
marker.scale.y = 1.0
marker.scale.z = 1.0
marker.color.r = 0.0
marker.color.g = 100.0
marker.color.b = 0.0
marker.color.a = 0.7
marker_array.markers.append(marker)
self.markerArray_publisher.publish(marker_array)
r.sleep()
def computePlan(self):
if (self.robotPos != None and self.goalPos != None):
start = time.time()
startGridCell = GridCell(self.robotPos.x, self.robotPos.y, None)
priorityQueue = PriorityQueue()
closeList = {}
openList = {}
openList[startGridCell.getKey()] = startGridCell
self.computeTotalCost(startGridCell)
priorityQueue.push(startGridCell.getCost(), startGridCell)
goal = None
while (len(openList) > 0):
(cost, currentCell) = priorityQueue.pop()
#print "low cell:(", currentCell.x , currentCell.y, ") cost:", cost
if (currentCell.getKey() in openList):
del openList[currentCell.getKey()]
if (self.isGoalGridCell(currentCell)):
goal = currentCell
break
else:
adjacentCells = self.getAdjacentCells(currentCell)
#self.printAdjacentCells(adjacentCells)
#print "\n"
for index in range(len(adjacentCells)):
adjacentCell = adjacentCells[index]
allReadyVisited = adjacentCell.getKey() in closeList
if (allReadyVisited): continue
self.computeTotalCost(adjacentCell)
#print "Adj:(" , adjacentCell.x , adjacentCell.y , ") " , "cost:", adjacentCell.getCost()
exploredBefore = adjacentCell.getKey() in openList
if (exploredBefore == False):
openList[adjacentCell.getKey()] = adjacentCell
priorityQueue.push(adjacentCell.getCost(),
adjacentCell)
#print "Adj:(" , adjacentCell.x , adjacentCell.y , ") ", "not in open list, added to open list"
else:
oldAdjacentCell = openList[adjacentCell.getKey()]
#print "found old Adj:(" , oldAdjacentCell.x , oldAdjacentCell.y , ") " , "cost:", oldAdjacentCell.getCost(), " parent:" , oldAdjacentCell.parentGridCell.x , oldAdjacentCell.parentGridCell.y
if (adjacentCell.getCost() <
oldAdjacentCell.getCost()):
oldAdjacentCell.setParent(currentCell)
self.computeTotalCost(oldAdjacentCell)
#print "old Adj upadte:(" , oldAdjacentCell.x , oldAdjacentCell.y , ") " , "new cost:", oldAdjacentCell.getCost(), " new parent:" , oldAdjacentCell.parentGridCell.x , oldAdjacentCell.parentGridCell.y
#else:
#print "Old Adj not updated"
#print "\n"
closeList[currentCell.getKey()] = currentCell
self.publisMultipleMarkers(closeList)
#print "low cell:(", currentCell.x , currentCell.y, ") cost:", cost, " REMOVED"
#print "-----------"
#print "Goal:", goal.x, goal.y
end = time.time()
elapsed = end - start
s = "Time taken: " + str(elapsed) + " seconds.\n"
s += "Total node expanded: " + str(
len(closeList)) + " out of " + "(30 * 30) = 900. \n"
self.path = []
if (goal is not None):
self.getPath(goal)
self.publisDrawPathLine()
else:
s = "Goal not found !!!"
self.publisText(s)
def publisRobotPoseMarker(self):
marker = Marker()
shape = Marker.SPHERE
marker.header.frame_id = '/map'
marker.header.stamp = rospy.Time()
marker.ns = 'AStartPlanning'
marker.id = 1
marker.type = shape
marker.action = Marker.ADD
marker.pose.position.x = self.robotPos.x + 0.5
marker.pose.position.y = self.robotPos.y + 0.5
marker.pose.position.z = 1.0
marker.scale.x = 0.75
marker.scale.y = 0.75
marker.scale.z = 1.0
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 0.0
marker.color.a = 0.75
self.markerPublisher.publish(marker)
def makePalnning(self):
r = rospy.Rate(5)
print 'Running A start planner'
self.publisRobotPoseMarker()
while not rospy.is_shutdown():
if self.updatePlanning:
self.computePlan()
self.updatePlanning = False
r.sleep()
class World:
# --------
# init: creates a an empty world
# with the given size in [m]
#
def __init__(self, width, height):
# World size:
self._width = width
self._height = height
# Border lines:
self._xll = 0.0 # x coord of left lower point
self._yll = 0.0 # y coord of left lower point
self._lines = [] # List of lines
ll = Point(self._xll, self._yll) # lower left
lr = Point(self._xll+width, self._yll) # lower right
ur = Point(self._xll+width, self._yll+height) # upper right
ul = Point(self._xll, self._yll+height) # upper left
self._lines.append(Line(ll,lr))
self._lines.append(Line(lr,ur))
self._lines.append(Line(ur,ul))
self._lines.append(Line(ul,ll))
# Boxes:
self._boxes = []
self._boxSensor = True
self._boxSensorAngle = 140 * (pi/180) # 140 degree
self._boxRadius = 0.1 # box radius
self._boxesSensedDist = [] # Distance to the sensed boxes
self._boxesSensedAngles = [] # Angles of the sensed boxes
# Rooms
self._rooms = []
# Define graphic window with coordinates and draw borderline
self._win = GraphWin("HTWG Robot Simulator", int(800.0*width/height), 800, autoflush=False)
self._win.setCoords(self._xll-0.1, self._yll-0.3, self._xll+width+0.1, self._yll+height+0.1)
for l in self._lines:
l.draw(self._win)
# Robot (initialization is done in setRobot()
self._robot = None
self._robotCircle = None # robot is shown as circle with robot position as center
self._robotTheta = None # robot's global orientation
self._robotLine = None # local x-axis of the robot
# Sensor values:
self._sensorShow = True # Sensor values are shown
self._sensorDist = [] # Distances to obstacles
self._sensorPoints = [] # Obstacle points
self._sensorLines = [] # Sensor beams as lines for drawing
# Clock:
self._clockTime = 0.0
p = Point(self._xll+width/2, self._yll-0.1)
self._clockTimeText = Text(p,"Clock Time %4.2f" % self._clockTime )
self._clockTimeText.draw(self._win)
# Occupancy Grid:
self._grid = None
# Path history
self._showPathHistory = False
self._drivenDistance = 0.0
# Drawn Polyline:
self._drawnPolyline = []
# --------
# Draw a polyline.
#
def drawPolyline(self, poly, color = 'green'):
self.undrawPolyline()
for n in range(len(poly)-1):
l = Line(Point(poly[n][0],poly[n][1]),Point(poly[n+1][0],poly[n+1][1]))
l.draw(self._win)
l.setFill(color)
l.setWidth(3)
self._drawnPolyline.append(l)
# --------
# Undraw the polyline.
#
def undrawPolyline(self):
if self._drawnPolyline == []:
return
for l in self._drawnPolyline:
l.undraw()
self._drawnPolyline = []
# --------
# add new a new line from point (x0,y0) to (x1,y1)
#
def addLine(self, x0, y0, x1, y1):
l = Line(Point(x0,y0),Point(x1,y1))
self._lines.append(l)
l.setWidth(5)
l.setFill('blue')
l.draw(self._win)
# --------
# add new a new round Box at point (x,y).
#
def addBox(self, x, y):
box = Circle(Point(x,y),self._boxRadius)
box.draw(self._win)
self._boxes.append(box)
# --------
# Define a new room with name n and center position (x,y).
#
def defineRoom(self, n, x, y):
self._rooms.append([n,x,y])
t = Text(Point(x,y),n)
t.draw(self._win)
# --------
# Return all rooms.
#
def getRooms(self):
return self._rooms
# --------
# set the robot at pose (x,y,theta) and draw it.
#
def setRobot(self, robot, x, y, theta):
self._robot = robot
robot.setWorld(self) # the robot must know his world
# Set robot and draw robot:
c = Point(x,y)
r = robot.getSize()/2
self._robotCircle = Circle(c,r)
self._robotTheta = theta
p = Point(x+r*cos(theta),y+r*sin(theta))
self._robotLine = Line(c,p) # line shows the robot's orientation
self._robotCircle.draw(self._win)
self._robotLine.draw(self._win)
self._robotLine.setWidth(3)
# Update status bar
self._clockTimeText.setText("Clock Time: %4.2f Driven Distance: %4.2f Position: %4.2f, %4.2f, %4.2f "
% (self._clockTime, self._drivenDistance, x, y, theta*180/pi))
# Show all:
self._udateWindow()
print "click in window to start"
self._win.getMouse() # pause for click in window
#k = self.win.getKey()
#print "key "
# --------
# get the true robot pose (x,y,theta).
#
def getTrueRobotPose(self):
x = self._robotCircle.getCenter().getX()
y = self._robotCircle.getCenter().getY()
theta = self._robotTheta
return [x,y,theta]
# --------
# move the robot in the direction of his self._robotTheta + dTheta/2 by the length of d
# and then change the robot's orientation by dTheta.
# The movement takes dt in clock time (dt is only used to change clock time output).
# If the robot's movement is not possible because of obstacles, the movement will be not
# performed and False is returned.
#
def moveRobot(self, d, dTheta, dT):
c = self._robotCircle.getCenter()
r = self._robotCircle.getRadius()
x = c.getX()
y = c.getY()
theta = self._robotTheta
dx = d*cos(theta+0.5*dTheta)
dy = d*sin(theta+0.5*dTheta)
nc = Point(x+dx,y+dy)
if self.getNearestDistance(nc) < r: # movement is not possible because of obstacles
print "Robot stalled: ", x, y, theta
# raw_input("Enter: ")
return False
# move robot and draw robot:
self._robotLine.undraw()
self._robotCircle.move(dx,dy)
self._robotTheta = (self._robotTheta + dTheta)%(2*pi)
p = Point(x+dx+r*cos(self._robotTheta),y+dy+r*sin(self._robotTheta))
self._robotLine = Line(nc,p)
self._robotLine.draw(self._win)
self._robotLine.setWidth(3)
# Path history:
self._drivenDistance += d
if self._showPathHistory == True:
pathLine = Line(c,nc)
pathLine.setFill('red')
pathLine.setWidth(3)
pathLine.draw(self._win)
#print x+dx, y+dy, self.robotTheta
# Clear sensor values, compute new sensor values and draw it:
self._sensorPoints = []
self._sensorDist = []
self.sense()
self._drawSense()
self._boxesSensedDist = []
self._boxesSensedAngles = []
self.senseBox()
# Update clock and status bar
self._clockTime += dT
self._clockTimeText.setText("Clock Time: %4.2f Driven Distance: %4.2f Position: %4.2f, %4.2f, %4.2f "
% (self._clockTime, self._drivenDistance, x+dx, y+dy, self._robotTheta*180/pi))
# show all
self._udateWindow()
return True
# --------
# Compute distance values in the given direction of the robot sensors
# If sensorShow = True, sensor beams are displayed.
#
def sense(self):
if self._sensorDist == []:
alphas = self._robot.getSensorDirections()
distMax = self._robot.getMaxSenseValue()
p = self._robotCircle.getCenter()
for alpha in alphas:
theta = (self._robotTheta+alpha) % (2*pi)
q = self.getNearestIntersectionWithBeam(p,theta)
#print "p: ", p.getX(), p.getY(), theta
d = World._dist(p,q)
#print "q: ", q.getX(), q.getY(), d
if d > distMax:
self._sensorDist.append(None)
x = p.getX()+distMax*cos(theta)
y = p.getY()+distMax*sin(theta)
self._sensorPoints.append(Point(x,y))
#print "sensorPoint: ", x, y, "\n"
else:
self._sensorDist.append(d)
self._sensorPoints.append(q)
#print "sensorPoint: ", q.getX(), q.getY(), "\n"
self._drawSense()
# print "time: ", self._clockTime, time.clock()
return self._sensorDist
# --------
# Draw sensor beams.
#
def _drawSense(self):
if not self._sensorShow:
return
# Undraw sensor beam lines:
for l in self._sensorLines:
l.undraw()
# Draw new sensor beam lines:
self._sensorLines = []
p = self._robotCircle.getCenter()
for q in self._sensorPoints:
l = Line(p,q)
l.setFill('red')
self._sensorLines.append(l)
l.draw(self._win)
self._robotCircle.undraw()
self._robotLine.undraw()
self._robotCircle.draw(self._win)
self._robotLine.draw(self._win)
self._robotLine.setWidth(3)
# --------
# If boxSensor = True, try to detect box and
# compute distance and orientation to the detected boxes.
#
def senseBox(self):
if self._boxSensor == False:
return None
if self._boxesSensedDist == []:
p = self._robotCircle.getCenter()
for box in self._boxes:
box.undraw()
box.setFill('white')
pb = box.getCenter()
theta = atan2(pb.getY()-p.getY(), pb.getX()-p.getX())
# print 'Box check', self._robotTheta, theta
# angle to box relative to robot's x axis from [-pi,+pi)
alphaBox = (self._robotTheta - theta + pi) % (2*pi) - pi
if abs(alphaBox) <= self._boxSensorAngle/2:
ip = self.getNearestIntersectionWithBeam(p, theta)
d = World._dist(p, pb)
if World._dist(p, ip) > d:
# Box can be seen:
#print 'Box can be seen', d, alphaBox
box.setFill('red')
self.boxDist = World._dist(p, pb)
self._boxesSensedDist.append(d)
self._boxesSensedAngles.append(alphaBox)
box.draw(self._win)
#print "senseBox: ", self._boxesSensedDist,self._boxesSensedAngles
return [self._boxesSensedDist,self._boxesSensedAngles]
def getCursorController(self):
return CursorController(self._win)
# --------
# update and draw the actual window.
# If simulation runs to fast then delay wth a time.sleep()
def _udateWindow(self):
#time.sleep(0.05)
self._win.update()
#self.win.getMouse() # pause for click in window
def close(self, waitForClick = True):
if waitForClick:
print "click in window to close"
self._win.getMouse() # pause for click in window
self._win.close()
# --------
# compute the nearest intersection point between the beam starting at point p in direction of theta
# and a line of the world
#
def getNearestIntersectionWithBeam(self, p, theta):
#print "\ntheta= ", theta*180/pi
if len(self._lines) == 0:
return None
dmin = float("inf")
ip = None
for line in self._lines:
#print "line: ", line.getP1().getX(),line.getP1().getY(),line.getP2().getX(),line.getP2().getY()
q = World._intersectSegmentBeam(p,theta,line)
if q is not None:
#print "q =", q.getX(), q.getY(), self._dist(p,q)
d = World._dist(p,q)
if d < dmin:
dmin = d
ip = q
if ip is None:
return None
#l = Line(p,ip)
#l.setFill('red')
#l.draw(self.win)
return ip
# --------
# compute the distance to the line of the world which is nearest to p.
#
def getNearestDistance(self, p):
if len(self._lines) == 0:
return None
dmin = float("inf")
for l in self._lines:
d = World._distPointSegment(p,l)
#print "Distance to Line: ", l.getP1().getX(), l.getP1().getY(), l.getP2().getX(), l.getP2().getY(), d
if d < dmin:
dmin = d
return dmin
# --------
# compute the distance between the points p and q.
#
@staticmethod
def _dist(p, q):
dx = p.getX()-q.getX()
dy = p.getY()-q.getY()
return sqrt(dx*dx+dy*dy)
# --------
# compute the distance between point p and segment line.
#
@staticmethod
def _distPointSegment(p, line):
p1 = line.getP1()
p2 = line.getP2()
x1 = p1.getX()
y1 = p1.getY()
x2 = p2.getX()
y2 = p2.getY()
theta = atan2(y2-y1,x2-x1)+pi/2
ip = World._intersectSegmentBeam(p, theta, line, oppositeDirectionInclusive = True)
if ip is None:
# print "ip = None: "
d1 = World._dist(p,p1)
d2 = World._dist(p,p2)
if d1 <= d2:
return d1
else:
return d2
else:
# print "ip: ", ip.getX(), ip.getY()
return World._dist(ip,p)
# --------
# Compute the intersection point between segment line and the beam given by p and theta.
# If oppositeDirectionInclusive = True, then the intersection point between segment line
# and the line through point p with grade theta is computed.
#
@staticmethod
def _intersectSegmentBeam(p, theta, line, oppositeDirectionInclusive = False):
x0 = p.getX()
y0 = p.getY()
x1 = line.getP1().getX()
y1 = line.getP1().getY()
x2 = line.getP2().getX()
y2 = line.getP2().getY()
delta = fabs(atan2(y2-y1,x2-x1)-theta)
if delta < 1.0e-03 or fabs(delta-pi) < 1.0e-03:
# line and beam are nearly parallel
# print 'parallel'
return None
a = np.array([[x2-x1, -cos(theta)],
[y2-y1, -sin(theta)]])
b = np.array([x0-x1,
y0-y1])
k = np.linalg.solve(a, b)
k0 = k[0]
k1 = k[1]
if k0 < 0 or k0 > 1:
return None
if not oppositeDirectionInclusive and k1 < 0:
return None
ip = Point(x1 + k0*(x2-x1), y1 + k0*(y2-y1))
return ip
def getOccupancyGrid(self, cellSize = 0.1):
if self._grid is None:
self._grid = OccupancyGrid(self._xll, self._yll, self._width, self._height, cellSize)
for l in self._lines:
x0 = l.getP1().getX()
y0 = l.getP1().getY()
x1 = l.getP2().getX()
y1 = l.getP2().getY()
self._grid.addLine(x0, y0, x1, y1)
return self._grid
# # Timing
# from timeit import timeit
#
# def timing():
# astar(start_pose, goal_pose, occupancy_grid)
#
# times = []
# for i in range(100):
# occupancy_grid, start_pose, goal_pose = OccupancyGrid.loadMap(grid)
# times.append(timeit(timing, number=1))
# times = np.array(times)
# print(np.mean(times))
# print(np.std(times))
occupancy_grid, start_pose, goal_pose = OccupancyGrid.loadMap(grid)
path = astar(start_pose, goal_pose, occupancy_grid)
path.append(goal_pose)
filtered_path = los_trimming(occupancy_grid, path)
linx, liny = zip(*filtered_path)
edited_path = smoothing(occupancy_grid, filtered_path, start_pose)
px, py = zip(*edited_path)
chain = CatmullRomChain(occupancy_grid, edited_path)
ax, ay = zip(*edited_path)
# Combine discretised splines to form a path
if chain:
class AStarPlanning:
def __init__(self):
self.robotPos = Position(0,0)
self.goalPos = None
self.occupancy_grid = OccupancyGrid()
self.occupancy_grid.initializeData()
self.subscribeEvents()
self.setMarkerPublishers()
self.markerUniqueId = 10
self.pathMarkerIds = []
self.openListMarkerIds = {}
def getMarkerUniqueId(self):
self.markerUniqueId +=1
return self.markerUniqueId
def setMarkerPublishers(self):
self.markerPublisher = rospy.Publisher('/planning_markers', Marker)
self.markerArray_publisher = rospy.Publisher('/planning_multiple_markers', MarkerArray, latch=True)
def subscribeEvents(self):
rospy.Subscriber('/goal_pos', PoseStamped, self.rvizGoalPosCallBack)
def rvizGoalPosCallBack(self, clickedPose):
comandType = rospy.get_param('comandType')
pos_x = int(clickedPose.pose.position.x)
pos_y = int(clickedPose.pose.position.y)
if(comandType == "o"):
clickedPosition = Position(pos_x, pos_y)
self.occupancy_grid.setData(clickedPosition.x, clickedPosition.y, 100)
elif(comandType == "g"):
r = rospy.Rate(4)
self.publisRobotPoseMarker()
self.deletePathMarkers()
r.sleep()
self.updatePlanning = True
self.goalPos = Position(pos_x, pos_y)
def deletePathMarkers(self):
for i in range(len(self.pathMarkerIds)):
pathMarkerId = self.pathMarkerIds[i]
marker = Marker()
marker.header.frame_id = '/map'
marker.ns = 'AStartPlanning'
marker.id = pathMarkerId
marker.action = Marker.DELETE
self.markerPublisher.publish(marker)
def publisText(self, s):
marker = Marker()
shape = Marker.TEXT_VIEW_FACING
marker.header.frame_id = '/map'
marker.header.stamp = rospy.Time() #rospy.Time.now()
marker.ns = 'AStartPlanning'
marker.id = 5
marker.type = shape
marker.action = Marker.ADD
marker.text = s
marker.pose.position.x = 17.0
marker.pose.position.y = -3.0
marker.pose.position.z = 2.0
marker.scale.z = 1.1
marker.color.r = 0.8
marker.color.g = 0.0
marker.color.b = 0.0
marker.color.a = 0.9
self.markerPublisher.publish(marker)
def publisDrawPathLine(self):
self.path.reverse()
r = rospy.Rate(5)
for i in range(len(self.path) - 1):
pathPoint1 = self.path[i]
pathPoint2 = self.path[i+1]
self.robotPos = Position(int(pathPoint2.x), int(pathPoint2.y))
marker = Marker()
shape = Marker.LINE_STRIP
marker.header.frame_id = '/map'
marker.header.stamp = rospy.Time() #rospy.Time.now()
marker.ns = 'AStartPlanning'
marker.id = self.getMarkerUniqueId()
self.pathMarkerIds.append(marker.id)
marker.type = shape
marker.action = Marker.ADD
marker.points = [pathPoint1, pathPoint2]
marker.scale.x = 0.4
marker.scale.y = 1.0
marker.scale.z = 1.0
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 1.0
marker.color.a = 1.0
self.publisRobotPoseMarker()
self.markerPublisher.publish(marker)
r.sleep()
def robotPosChangedCallback(self, robotPos):
self.robotPos = robotPos
self.cmd_robotPos_sub.unregister()
def goalPosChangedCallback(self, goalPos):
self.goalPos = goalPos
def computeEquledianDistance(self, currentCell):
xDistance = self.goalPos.x - currentCell.x
yDistance = self.goalPos.y - currentCell.y
return math.sqrt( math.pow(xDistance, 2) + math.pow(yDistance, 2) )
def computeH(self, childGridCell):
childGridCell.hVal = self.computeEquledianDistance(childGridCell)
def oneCellMoveCost(self):
return 1;
def isAnObstacle(self, gridCell):
gridCellData = self.occupancy_grid.data[gridCell.x][gridCell.y]
if(gridCellData == 100):
return True
return False
def getCellCost(self, gridCell):
if(self.isAnObstacle(gridCell)):
return 100
return 1
def computeG(self, gridCell):
if(gridCell.parentGridCell is None):
gridCell.gVal = 0
else:
gridCell.gVal = gridCell.parentGridCell.gVal + self.getCellCost(gridCell)
def computeTotalCost(self, childGridCell):
self.computeG(childGridCell)
self.computeH(childGridCell)
return childGridCell.getCost()
def isGoalGridCell(self, currentCell):
return currentCell.x == self.goalPos.x and currentCell.y == self.goalPos.y
def isInRange(self, x, y):
return (x >= 0 and y >= 0) and ( x < self.occupancy_grid.dimension.width and y < self.occupancy_grid.dimension.height)
def getAdjacentCells(self, currentCell):
adjacentCells = []
x = currentCell.x
y = currentCell.y
if(self.isInRange(x + 1, y)):
gridCell = GridCell(x + 1, y,currentCell)
if(self.isAnObstacle(gridCell) == False):
adjacentCells.append(gridCell)
if(self.isInRange(x + 1, y + 1)):
gridCell = GridCell(x + 1, y + 1,currentCell)
if(self.isAnObstacle(gridCell) == False):
adjacentCells.append(gridCell)
if(self.isInRange(x, y + 1)):
gridCell = GridCell(x, y + 1,currentCell)
if(self.isAnObstacle(gridCell) == False):
adjacentCells.append(gridCell)
if(self.isInRange(x - 1, y + 1)):
gridCell = GridCell(x - 1, y + 1,currentCell)
if(self.isAnObstacle(gridCell) == False):
adjacentCells.append(gridCell)
if(self.isInRange(x - 1, y)):
gridCell = GridCell(x - 1, y,currentCell)
if(self.isAnObstacle(gridCell) == False):
adjacentCells.append(gridCell)
if(self.isInRange(x - 1, y - 1)):
gridCell = GridCell(x - 1, y - 1,currentCell)
if(self.isAnObstacle(gridCell) == False):
adjacentCells.append(gridCell)
if(self.isInRange(x, y - 1)):
gridCell = GridCell(x, y - 1, currentCell)
if(self.isAnObstacle(gridCell) == False):
adjacentCells.append(gridCell)
if(self.isInRange(x + 1, y - 1)):
gridCell = GridCell(x + 1, y - 1,currentCell)
if(self.isAnObstacle(gridCell) == False):
adjacentCells.append(gridCell)
return adjacentCells
def printAdjacentCells(self, adjs):
s = "Adjs : "
for i in range(len(adjs)):
adj = adjs[i]
s += "(" + str(adj.x) + "," + str(adj.y) + ") "
print s
def getPath(self, gridCell):
self.path.append(Point(gridCell.x + 0.5 ,gridCell.y + 0.5, 1))
if(gridCell.x == self.robotPos.x and gridCell.y == self.robotPos.y):
return
self.getPath(gridCell.parentGridCell)
def publisMultipleMarkers(self, dictionary):
l = dictionary.items()
marker_array = MarkerArray()
r = rospy.Rate(25)
life = 6
for i in range(len(l)):
(key, gridCell) = l[i]
marker = Marker()
marker.header.frame_id = '/map'
marker.header.stamp = rospy.Time()
marker.ns = 'AStartPlanning'
markerId = str(gridCell.x) +"_" + str(gridCell.y)
if(markerId in self.openListMarkerIds):
marker.id = self.openListMarkerIds[markerId]
else:
marker.id = self.getMarkerUniqueId()
self.openListMarkerIds[markerId] = marker.id
marker.type = Marker.CUBE
marker.action = Marker.ADD
marker.lifetime = rospy.Duration(life, 0)
marker.pose.position.x = gridCell.x + 0.5
marker.pose.position.y = gridCell.y + 0.5
marker.scale.x = 1.0
marker.scale.y = 1.0
marker.scale.z = 1.0
marker.color.r = 0.0
marker.color.g = 100.0
marker.color.b = 0.0
marker.color.a = 0.7
marker_array.markers.append(marker)
self.markerArray_publisher.publish(marker_array)
r.sleep()
def computePlan(self):
if(self.robotPos != None and self.goalPos != None):
start = time.time()
startGridCell = GridCell(self.robotPos.x, self.robotPos.y, None)
priorityQueue = PriorityQueue()
closeList = {}
openList = {}
openList[startGridCell.getKey()] = startGridCell
self.computeTotalCost(startGridCell)
priorityQueue.push(startGridCell.getCost(),startGridCell)
goal = None
while(len(openList) > 0):
(cost, currentCell) = priorityQueue.pop()
#print "low cell:(", currentCell.x , currentCell.y, ") cost:", cost
if(currentCell.getKey() in openList):
del openList[currentCell.getKey()]
if(self.isGoalGridCell(currentCell)):
goal = currentCell
break
else:
adjacentCells = self.getAdjacentCells(currentCell)
#self.printAdjacentCells(adjacentCells)
#print "\n"
for index in range(len(adjacentCells)):
adjacentCell = adjacentCells[index]
allReadyVisited = adjacentCell.getKey() in closeList
if(allReadyVisited): continue
self.computeTotalCost(adjacentCell)
#print "Adj:(" , adjacentCell.x , adjacentCell.y , ") " , "cost:", adjacentCell.getCost()
exploredBefore = adjacentCell.getKey() in openList
if(exploredBefore == False):
openList[adjacentCell.getKey()] = adjacentCell
priorityQueue.push(adjacentCell.getCost(),adjacentCell)
#print "Adj:(" , adjacentCell.x , adjacentCell.y , ") ", "not in open list, added to open list"
else:
oldAdjacentCell = openList[adjacentCell.getKey()]
#print "found old Adj:(" , oldAdjacentCell.x , oldAdjacentCell.y , ") " , "cost:", oldAdjacentCell.getCost(), " parent:" , oldAdjacentCell.parentGridCell.x , oldAdjacentCell.parentGridCell.y
if(adjacentCell.getCost() < oldAdjacentCell.getCost()):
oldAdjacentCell.setParent(currentCell)
self.computeTotalCost(oldAdjacentCell)
#print "old Adj upadte:(" , oldAdjacentCell.x , oldAdjacentCell.y , ") " , "new cost:", oldAdjacentCell.getCost(), " new parent:" , oldAdjacentCell.parentGridCell.x , oldAdjacentCell.parentGridCell.y
#else:
#print "Old Adj not updated"
#print "\n"
closeList[currentCell.getKey()] = currentCell
self.publisMultipleMarkers(closeList)
#print "low cell:(", currentCell.x , currentCell.y, ") cost:", cost, " REMOVED"
#print "-----------"
#print "Goal:", goal.x, goal.y
end = time.time()
elapsed = end - start
s = "Time taken: " + str(elapsed) + " seconds.\n"
s += "Total node expanded: " + str(len(closeList)) + " out of " + "(30 * 30) = 900. \n"
self.path = []
if(goal is not None):
self.getPath(goal)
self.publisDrawPathLine()
else:
s = "Goal not found !!!"
self.publisText(s)
def publisRobotPoseMarker(self):
marker = Marker()
shape = Marker.SPHERE
marker.header.frame_id = '/map'
marker.header.stamp = rospy.Time()
marker.ns = 'AStartPlanning'
marker.id = 1
marker.type = shape
marker.action = Marker.ADD
marker.pose.position.x = self.robotPos.x + 0.5
marker.pose.position.y = self.robotPos.y + 0.5
marker.pose.position.z = 1.0
marker.scale.x = 0.75
marker.scale.y = 0.75
marker.scale.z = 1.0
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 0.0
marker.color.a = 0.75
self.markerPublisher.publish(marker)
def makePalnning(self):
r = rospy.Rate(5)
print 'Running A start planner'
self.publisRobotPoseMarker()
while not rospy.is_shutdown():
if self.updatePlanning:
self.computePlan()
self.updatePlanning = False
r.sleep()
def __init__(self):
self.agent_data = {}
self.occupancy_grid = occ.OccupancyGrid()
