def iterate(self):
global plotter
curPos = self.curPos
heading = self.heading
cloud = self.cloud
goalPos = self.goalPos
curTwist = self.curTwist
msg = Twist()
if len(self.pdata.ranges) == 0:
print 'got invalid data from GetPlanarData service...',\
'so um is anything being published to /planar_data?'
return msg
if goalPos.z == TIMEOUT_ERROR:
# this indicates a goal data timeout in the DataServiceProvider
# making us stop if GoalMaker has stopped publishing to /goal
rospy.loginfo("stopping because of a goal data timeout")
return msg
distToGoal = GLib.euclid(goalPos.x, goalPos.y, curPos.x, curPos.y)
print "distance to goal:", distToGoal
if distToGoal < CLOSE_ENOUGH_TO_GOAL:
rospy.loginfo("stopping because we're close enough to the goal")
return msg
if plotter:
plotEverything(
curPos.x,
curPos.y,
heading,
goalPos.x,
goalPos.y,
cloud
)
(msg.angular.z, msg.linear.x) = calcDynamicWindow(
curPos.x,
curPos.y,
heading,
curTwist.linear.x,
curTwist.angular.z,
cloud,
goalPos.x,
goalPos.y,
1/RATE,
plotter
)
return msg
def step():
global cur_x, cur_y, cur_dir, cur_lin, cur_ang, goal_x, goal_y
plotEverything()
mycloud = []
for cloudPoint in cloud:
if GLib.euclid(cloudPoint.x, cloudPoint.y, cur_x, cur_y) < CLEARANCE_MAX:
mycloud.append(cloudPoint);
(cur_ang, cur_lin) = calcDynamicWindow(
cur_x,
cur_y,
cur_dir,
cur_lin,
cur_ang,
mycloud,
goal_x,
goal_y,
DT,
plotter
)
kine = GLib.calcTrajectoryStepFromTime(
cur_x,
cur_y,
cur_dir,
cur_lin,
cur_ang,
DT
)
cur_x = kine[0]
cur_y = kine[1]
cur_dir = kine[2]
plotter.plotArrow(cur_x, cur_y, cur_dir, "red")
plotter.display()
def step():
global cur_x, cur_y, cur_dir, cur_lin, cur_ang, goal_x, goal_y
plotEverything()
mycloud = []
for cloudPoint in cloud:
if GLib.euclid(cloudPoint.x, cloudPoint.y, cur_x,
cur_y) < CLEARANCE_MAX:
mycloud.append(cloudPoint)
(cur_ang, cur_lin) = calcDynamicWindow(cur_x, cur_y, cur_dir, cur_lin,
cur_ang, mycloud, goal_x, goal_y,
DT, plotter)
kine = GLib.calcTrajectoryStepFromTime(cur_x, cur_y, cur_dir, cur_lin,
cur_ang, DT)
cur_x = kine[0]
cur_y = kine[1]
cur_dir = kine[2]
plotter.plotArrow(cur_x, cur_y, cur_dir, "red")
plotter.display()
def circleIntersections(c1, c2):
dist = GLib.euclid(c1.x, c1.y, c2.x, c2.y)
angle = math.atan2(c2.y - c1.y, c2.x - c1.x)
small = c1.r if (c1.r < c2.r) else c2.r
big = c1.r if (c1.r > c2.r) else c2.r
if c1.r == c2.r and c1.x == c2.x and c1.y == c2.y:
return []
elif dist == small + big or dist == big - small:
return [Point(c1.x + c1.r*math.cos(angle),
c1.y + c1.r*math.sin(angle))]
elif dist < small + big and dist > big - small:
# using the law of cosines
angleOffset = math.acos((c1.r*c1.r + dist*dist - c2.r*c2.r)/(2.0*c1.r*dist))
return [
Point(c1.x + c1.r*math.cos(angle + angleOffset),
c1.y + c1.r*math.sin(angle + angleOffset)),
Point(c1.x + c1.r*math.cos(angle - angleOffset),
c1.y + c1.r*math.sin(angle - angleOffset))
]
return []
def calcDynamicWindow(
cur_x,
cur_y,
cur_dir,
cur_linear,
cur_angular,
mycloud,
goal_x,
goal_y,
deltaTime,
plotter=None
):
distToGoal = GLib.euclid(goal_x, goal_y, cur_x, cur_y)
if distToGoal < CLOSE_ENOUGH_TO_GOAL:
return DO_NOTHING
best_weight = -1
best_linear = None
best_angular = None
best_found = False
for angular in angularArr:
if angular < cur_angular - ANGULAR_ACCEL or \
angular > cur_angular + ANGULAR_ACCEL:
continue
for linear in linearArr:
if linear < cur_linear - LINEAR_ACCEL or \
linear > cur_linear + LINEAR_ACCEL:
continue
#
# get clearance and normalize
#
res = CLib.calcIntersection(
cur_x,
cur_y,
cur_dir,
linear,
angular,
mycloud
)
clearance = CLEARANCE_MAX
color = "gray"
if res.point and res.delta < CLEARANCE_MAX:
clearance = res.delta
color = "red"
if clearance < CLEARANCE_MIN:
continue
clearanceNorm = clearance/CLEARANCE_MAX
#
# get normalized goal direction weight
#
newDir = cur_dir + angular*deltaTime
goalDir = math.atan2(goal_y - cur_y, goal_x - cur_x)
goalDirDif = math.pi - abs(GLib.angleDif(goalDir, newDir))
goalDirDifNorm = goalDirDif/math.pi
#
# get normalized linear velocity weight
#
linearNorm = linear/LINEAR_MAX
#
# get the final weight, compare it with what we've seen so far
#
weight = clearanceNorm*CLEARANCE_WEIGHT + \
goalDirDifNorm*GOAL_DIR_WEIGHT + \
linearNorm*LINEAR_WEIGHT
if weight > best_weight:
best_weight = weight
best_linear = linear
best_angular = angular
best_found = True
#
# plot trajectory & intersection point, if there was one
#
if plotter:
if res.point:
plotter.plotPoint(res.point.x, res.point.y, .1, color)
if res.traj.type == "circle":
plotter.plotCircle(res.traj.x, res.traj.y, res.traj.r, color)
elif res.traj.type == "vector":
plotter.plotVector(res.traj.x, res.traj.y, res.traj.dir, color)
if not best_found or (abs(best_linear) <= 1e-6 and abs(best_angular) <= 1e-6):
return DO_NOTHING
if distToGoal < SLOW_DOWN_RADIUS:
best_linear *= (distToGoal/SLOW_DOWN_RADIUS)
return (best_angular, best_linear)
def trajectoryIntersection(pose, traj, circle):
if traj.type == "circle":
points = CLib.circleIntersections(traj, circle)
if len(points) == 0:
return False
elif len(points) == 1:
return points[0]
elif len(points) == 2:
# need to find the closest of the two points to the starting point
# along the trajectory
curAngle = GLib.boundAngle0to2Pi(math.atan2(
pose.y - traj.y,
pose.x - traj.x
))
angle1 = GLib.boundAngle0to2Pi(math.atan2(
points[0].y - traj.y,
points[0].x - traj.x
))
angle2 = GLib.boundAngle0to2Pi(math.atan2(
points[1].y - traj.y,
points[1].x - traj.x
))
if pose.w > 0:
angle1 = angle1 + math.pi*2 if (angle1 < curAngle) else angle1
angle2 = angle2 + math.pi*2 if (angle2 < curAngle) else angle2
if angle1 < angle2:
return TrajIntersection(
points[0],
traj.r*(angle1 - curAngle)
)
else:
return TrajIntersection(
points[1],
traj.r*(angle2 - curAngle)
)
else:
angle1 = angle1 - math.pi*2 if (angle1 > curAngle) else angle1
angle2 = angle2 - math.pi*2 if (angle2 > curAngle) else angle2
if angle1 > angle2:
return TrajIntersection(
points[0],
traj.r*(curAngle - angle1)
)
else:
return TrajIntersection(
points[1],
traj.r*(curAngle - angle2)
)
elif traj.type == "vector":
vector = Vector(circle.x, circle.y, traj.dir + math.pi/2.0)
s = CLib.getParameterOfIntersection(
vector.x, vector.y, vector.dir,
traj.x, traj.y, traj.dir
)
# if the intersection doesn't occur or if it occurs in the
# opposite direction, return False
if False == s:
return False
intersection = Point(
traj.x + s*math.cos(traj.dir),
traj.y + s*math.sin(traj.dir)
)
# if the intersection occured inside the circle, check the real
# intersection on the circle's perimeter
a = GLib.euclid(intersection.x, intersection.y, circle.x, circle.y)
if a < circle.r:
r = circle.r
b = math.sqrt(r*r - a*a)
dist = GLib.euclid(intersection.x, intersection.y, traj.x, traj.y)
if s < 0:
dist *= -1
if dist - b > 0:
dist -= b
elif dist + b > 0:
dist += b
else:
return False
return TrajIntersection(
Point(
traj.x + dist*math.cos(traj.dir),
traj.y + dist*math.sin(traj.dir)
),
dist
)
return False
cur_y = 0
cur_dir = 0
goal_x = 5
goal_y = 5
cur_lin = 0
cur_ang = 0
cloud = []
if __name__ == '__main__':
print "everything compiles!"
for i in range(NUM_OBSTACLES):
while True:
x = random.random()*26-13
y = random.random()*14-7
if GLib.euclid(x, y, cur_x, cur_y) > 2.0*SIZE_RADIUS:
break
cloud.append(CloudPoint(
x,
y,
SIZE_RADIUS + BUFFER_SPACE
))
pygame.init()
window = pygame.display.set_mode((WINDOW_WIDTH, WINDOW_HEIGHT))
plotter = Plotter(window, WINDOW_WIDTH, WINDOW_HEIGHT, .5, .5, 50, 50)
plotEverything()
Thread(target=loopPlotter, args=[]).start()
def calcDynamicWindow(cur_x,
cur_y,
cur_dir,
cur_linear,
cur_angular,
mycloud,
goal_x,
goal_y,
deltaTime,
plotter=None):
distToGoal = GLib.euclid(goal_x, goal_y, cur_x, cur_y)
if distToGoal < CLOSE_ENOUGH_TO_GOAL:
return DO_NOTHING
best_weight = -1
best_linear = None
best_angular = None
best_found = False
for angular in angularArr:
if angular < cur_angular - ANGULAR_ACCEL or \
angular > cur_angular + ANGULAR_ACCEL:
continue
for linear in linearArr:
if linear < cur_linear - LINEAR_ACCEL or \
linear > cur_linear + LINEAR_ACCEL:
continue
#
# get clearance and normalize
#
res = CLib.calcIntersection(cur_x, cur_y, cur_dir, linear, angular,
mycloud)
clearance = CLEARANCE_MAX
color = "gray"
if res.point and res.delta < CLEARANCE_MAX:
clearance = res.delta
color = "red"
if clearance < CLEARANCE_MIN:
continue
clearanceNorm = clearance / CLEARANCE_MAX
#
# get normalized goal direction weight
#
newDir = cur_dir + angular * deltaTime
goalDir = math.atan2(goal_y - cur_y, goal_x - cur_x)
goalDirDif = math.pi - abs(GLib.angleDif(goalDir, newDir))
goalDirDifNorm = goalDirDif / math.pi
#
# get normalized linear velocity weight
#
linearNorm = linear / LINEAR_MAX
#
# compute the long-term goal proximity weight
#
if res.traj.type == "vector":
goaldist = abs(goal_y)
elif res.traj.type == "circle":
gd = GLib.euclid(goal_x, goal_y, res.traj.x, res.traj.y)
goaldist = abs(gd - res.traj.r)
elif res.traj.type == "point":
goaldist = GLib.euclid(goal_x, goal_y, cur_x, cur_y)
goaldist = GOAL_DIST_MAX if goaldist > GOAL_DIST_MAX else goaldist
goalDistNorm = goaldist / GOAL_DIST_MAX
#
# get the final weight, compare it with what we've seen so far
#
weight = clearanceNorm*CLEARANCE_WEIGHT + \
goalDirDifNorm*GOAL_DIR_WEIGHT + \
linearNorm*LINEAR_WEIGHT + \
goalDistNorm*GOAL_DIST_WEIGHT
if weight > best_weight:
best_weight = weight
best_linear = linear
best_angular = angular
best_found = True
#
# plot trajectory & intersection point, if there was one
#
if plotter:
if res.point:
plotter.plotPoint(res.point.x, res.point.y, .1, color)
if res.traj.type == "circle":
plotter.plotCircle(res.traj.x, res.traj.y, res.traj.r,
color)
elif res.traj.type == "vector":
plotter.plotVector(res.traj.x, res.traj.y, res.traj.dir,
color)
plotter.display()
if not best_found or (abs(best_linear) <= 1e-6
and abs(best_angular) <= 1e-6):
return DO_NOTHING
if distToGoal < SLOW_DOWN_RADIUS:
best_linear *= (distToGoal / SLOW_DOWN_RADIUS)
return (best_angular, best_linear)
cur_y = 0
cur_dir = 0
goal_x = 5
goal_y = 5
cur_lin = 0
cur_ang = 0
cloud = []
if __name__ == '__main__':
print "everything compiles!"
for i in range(NUM_OBSTACLES):
while True:
x = random.random() * 26 - 13
y = random.random() * 14 - 7
if GLib.euclid(x, y, cur_x, cur_y) > 2.0 * SIZE_RADIUS:
break
cloud.append(CloudPoint(x, y, SIZE_RADIUS + BUFFER_SPACE))
pygame.init()
window = pygame.display.set_mode((WINDOW_WIDTH, WINDOW_HEIGHT))
plotter = Plotter(window, WINDOW_WIDTH, WINDOW_HEIGHT, .5, .5, 50, 50)
plotEverything()
Thread(target=loopPlotter, args=[]).start()
while True:
for event in pygame.event.get():
print event
if event.type == QUIT:
