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: