class PathPlan(object): """ Listens for goal pose published by RViz and uses it to plan a path from current car pose. """ def __init__(self): self.odom_topic = rospy.get_param("~odom_topic") self.map_sub = rospy.Subscriber("/obstacles", OccupancyGrid, self.obstacle_map_cb) self.trajectory = LineTrajectory("/final_trajectory") self.traj_sub = rospy.Subscriber("/trajectory/current", PoseArray, self.trajectory_cb, queue_size=10) self.goal_sub = rospy.Subscriber("/move_base_simple/goal", PoseStamped, self.goal_cb, queue_size=10) self.traj_pub = rospy.Publisher("/trajectory/final", PoseArray, queue_size=10) self.odom_sub = rospy.Subscriber(self.odom_topic, Odometry, self.odom_cb, queue_size=10) self.rest_of_trajectory = LineTrajectory("/rest_of_trajectory") self.planning_dist = 50.0 # FOR TESTING # self.vis_pub = rospy.Publisher("/particles", Marker, queue_size=1) self.nearest_pub = rospy.Publisher("/nearest", Marker, queue_size=1) self.start_pose = None self.goal_pose = None #self.max_iterations = 1000 # Max size of RRT* tree before breaking. self.dim = 2 # Configuration space dimensionality ([x, y] -> 2, [x, y, theta] -> 3) self.gamma = 10 # I don't actually know what this is called lol, it's used in rrt_star.radius() self.eta = .75 self.goal_bias = 0.15 self.map = None # FOR TESTING PURPOSES ONLY -- set to true if you want to see the tree grow self.enable_vis = True def obstacle_map_cb(self, msg): """ Convert from OccupancyGrid to actual array. """ self.map = Map(msg, dilate=5) if True: rospy.loginfo("Obstacle map received! Origin: " + str(msg.info.origin)) self.pick_intermediate_goal(self.map) if True: rospy.loginfo("start pose: " + str(self.start_pose)) rospy.loginfo("end pose: " + str(self.end_pose)) self.start_time = rospy.get_time() self.time_thresh = 0.5 self.plan_path(self.start_pose, self.end_pose, self.map) self.extend_trajectory(self.rest_of_trajectory) # publish trajectory self.traj_pub.publish(self.trajectory.toPoseArray()) # visualize trajectory Markers self.trajectory.publish_viz() def extend_trajectory(self): """ adds rest of trajectory after intermediate points to the planned obstacle avoiding trajectory """ traj_poses = self.trajectory.toPoseArray() rest_poses = self.rest_of_trajectory.toPoseArray() self.trajectory.fromPoseArray(traj_poses.poses + rest_poses.poses) def pick_intermediate_goal(self): """ precondition: self.trajectory is populated w/new obstacle-agnostic trajectory takes in current planned path, and obstacle map (self.map) finds first segment within planning_dist that is blocked by obstacle + replans to endpoint of segment return false if we can just use current traj otherwise returns true&saves goal to self.end_pose, save rest of previous path to something else """ first_i, _ = self.get_closest_seg(self.start_pose) if first_i + 1 >= self.num_pts: raise Exception("something bad happened") if self.map.generate_line_path_unlimited( self.start_pose, self.trajectory.np_points[first_i + 1]): safe_pt = first_i + 1 while self.trajectory.distance_along_trajectory( safe_pt) - self.trajectory.distance_along_trajectory( first_i) < self.planning_dist: if safe_pt + 1 >= self.num_pts: return False if not self.map.generate_line_path_unlimited( self.trajectory.np_points[safe_pt], self.trajectory.np_points[safe_pt + 1]): self.end_pose = self.trajectory.np_points(safe_pt + 1) self.rest_of_trajectory.clear() poseArr = self.trajectory.toPoseArray() self.rest_of_trajectory.fromPoseArray( poseArr.poses[safe_pt + 2:]) return True safe_pt += 1 else: self.end_pose = self.trajectory.np_points(first_i + 1) self.rest_of_trajectory.clear() poseArr = self.trajectory.toPoseArray() self.rest_of_trajectory.fromPoseArray(poseArr.poses[first_i + 2:]) return True def trajectory_cb(self, msg): if self.trajectory.empty(): self.trajectory.fromPoseArray(msg) self.trajectory.make_np_array() #TODO: extend last pt?? self.num_pts = len(self.trajectory.np_points) def next_pt(i): return (i + 1) % self.num_pts # n entries, one for each line segment + connect the last to the first point self.trajectory_dists = np.array([ np.linalg.norm(self.trajectory.np_points[next_pt(i)] - self.trajectory.np_points[i])**2 for i in range(self.num_pts) ]) # also vectors bw points can be calculated once only self.wv = np.array([ self.trajectory.np_points[next_pt(i)] - self.trajectory.np_points[i] for i in range(self.num_pts) ]) def get_closest_seg(self, pose_np): """ given 1x2 numpy pose array, find closest segment from nx2 trajectory.np_points return 2 points from line segment (?) may need to be optimized let the point the robot is at be p, and for each line segment the start and end points be v and w respect~ """ pv = pose_np - self.trajectory.np_points[:last_pt] # self.num_pts x 2 # projecting p onto line bw vw, constraining b/w 0 and 1 to keep within line segment t = np.maximum( 0, np.minimum(1, (pv * self.wv[:last_pt]).sum(axis=-1) / self.trajectory_dists[:last_pt])) proj = self.trajectory.np_points[: last_pt] + self.wv[: last_pt] * t[:, np. newaxis] # return index of minimum distance, and closest pt minseg = np.argmin(np.linalg.norm(pose_np - proj, axis=1)) return minseg, proj[minseg] def odom_cb(self, msg): """ """ self.start_pose = np.array( [msg.pose.pose.position.x, msg.pose.pose.position.y]) def goal_cb(self, msg): self.end_pose = np.array([msg.pose.position.x, msg.pose.position.y]) if VERBOSE: rospy.loginfo("start pose: " + str(self.start_pose)) rospy.loginfo("end pose: " + str(self.end_pose)) self.start_time = rospy.get_time() self.time_thresh = 117. self.plan_path(self.start_pose, self.end_pose, self.map) def branched_from_existing_path(self, final_node, depth_underestimate=0): ''' depth_underestimate: bias to add to depth. Because we don't recursively update it, depth will steadily get less and less accurate as the path is denser than expected due to rewirings. Return a random free space that was near the existing path up to the final_node ''' def find_ancestor(node, n): if n <= 0 or node.parent is None: return node else: return find_ancestor(node.parent, n - 1) # back_track_depth = np.random.choice(int(final_node.depth + depth_underestimate)) back_track_depth = np.random.choice(int(final_node.depth)) x_temp = find_ancestor(final_node, back_track_depth) if x_temp.parent is None: # No point in exploring directly from the origin return self.map.sample_free_space() extend = 1 frac = .1 * extend while extend > 0: rand = np.random.uniform() * 2 * np.pi free_space_near_path = x_temp.value + extend * np.array( [np.cos(rand), np.sin(rand)]) if self.map.check_free(free_space_near_path): return free_space_near_path else: extend -= frac # unlucky -- just give a random free point return self.map.sample_free_space() def find_nearby_connectable(self, x_nearest, x_new): radius = self.rrt_star.radius(self.gamma, self.dim) X_nearby = self.rrt_star.nearest_in_n_sphere( x_new, 1) #min(radius, self.eta)) # Find all nodes near x_new # Find all X_near nodes that can actually be connected to x_new X_nearby_connectable = [x_nearest ] # List of candidate parent nodes for x_new for node in X_nearby: TEMP_PATH = self.map.generate_line_path_unlimited( node.value, x_new) if (TEMP_PATH is not None) and ( node != x_nearest ): # If the path exists between node and x_new, append node to list of candidate parents. (x_nearest starts in the list, so it's gonna be excluded here.) X_nearby_connectable.append(node) return X_nearby_connectable def find_best_parent(self, X_nearby_connectable, x_new): cost_min = np.inf node_min = None # Find which candidate parent node will impart the minimum cost to the x_new node when connected for node in X_nearby_connectable: TEMP_COST = node.cost + self.rrt_star.cost(node.value, x_new) if TEMP_COST < cost_min: cost_min = TEMP_COST node_min = node return cost_min, node_min def rewire(self, cost_min, node_new, X_nearby_connectable): # Rewire to minimize costs for node in X_nearby_connectable: potential_cost = cost_min + self.rrt_star.cost( node_new.value, node.value) if potential_cost < node.cost: self.rrt_star.adoption(node, node_new) def plan_path(self, start_point, end_point, map_obj): """ RRT*. Tries to find a collision free path from start to goal. """ # STUFF FOR TESTING self.trajectory.clear() if self.enable_vis: marker = Marker() marker.header.frame_id = "/map" marker.type = marker.POINTS marker.action = marker.ADD marker.scale.x = 0.1 marker.scale.y = 0.1 self.vis_pub.publish(marker) exploration_bias = 1.0 - self.goal_bias final_node = None num_existing_path_points_added = 0 self.rrt_star = RRTStar(Node(start_point)) self.max_iterations = self.rrt_star.max_size while self.rrt_star.size <= self.max_iterations: p = np.random.uniform() if p < exploration_bias: x_rand = self.map.sample_free_space() else: if final_node is None: x_rand = end_point else: x_rand = self.branched_from_existing_path( final_node, depth_underestimate=num_existing_path_points_added) num_existing_path_points_added += 1 x_nearest = self.rrt_star.nearest( x_rand) # Find the nearest node to x_rand path = self.map.generate_line_path(x_nearest.value, x_rand, eta=self.eta) if path is not None: # no obstacles between x_nearest and x_rand x_new = path[-1] X_nearby_connectable = self.find_nearby_connectable( x_nearest, x_new) cost_min, node_min = self.find_best_parent( X_nearby_connectable, x_new) X_nearby_connectable.remove( node_min ) # Remove x_new's parent node from the list of nearby nodes so it is not considered for rewiring # Create the new node at x_new! node_new = self.rrt_star.add_config(node_min, x_new) if self.enable_vis: # FOR TESTING ONLY # # Code to publish marker for new node ########################################################################################### TEMP = Point() TEMP.x = x_new[0] TEMP.y = x_new[1] TEMP.z = .05 marker.points.append(TEMP) TEMP = ColorRGBA() TEMP.r = 1 TEMP.g = 0 TEMP.b = 0 TEMP.a = 1 marker.colors.append(TEMP) self.vis_pub.publish(marker) ########################################################################################### self.rewire(cost_min, node_new, X_nearby_connectable) if np.allclose(node_new.value, end_point, .05, 0) and ( final_node is None): #np.array_equal(node_new.value, end_point): final_node = node_new # reduce exploration bias so that we reinforce the existing path exploration_bias = .5 if VERBOSE: print("Path found!!!!") print(final_node.cost) if rospy.get_time() - self.start_time > self.time_thresh: if VERBOSE: print(self.rrt_star.size) break if final_node is not None: if self.enable_vis: marker = Marker() marker.header.frame_id = "/map" marker.type = marker.POINTS marker.action = marker.ADD marker.scale.x = 0.1 marker.scale.y = 0.1 marker.points = [] marker.colors = [] def recur(node): if self.enable_vis: TEMP = Point() TEMP.x = node.value[0] TEMP.y = node.value[1] TEMP.z = .05 marker.points.append(TEMP) TEMP = ColorRGBA() TEMP.r = 1 TEMP.g = 0 TEMP.b = 0 TEMP.a = 1 marker.colors.append(TEMP) self.trajectory.points.append([node.value[0], node.value[1]]) parent = node.parent if parent is not None: recur(parent) recur(final_node) self.trajectory.points.reverse() if self.enable_vis: self.vis_pub.publish(marker) if VERBOSE: print(final_node.depth) else: # TODO:give best guess- find closest node to goal if VERBOSE: print("No path found! Please try again.")
class TrajectoryWithSpeedLoader(): def __init__(self): # Define Adjustaable Parameters self.min_speed = rospy.get_param("~min_speed") self.max_speed = rospy.get_param("~max_speed") self.max_acceleration = rospy.get_param("~max_acceleration") self.max_decceleration = rospy.get_param("~max_decceleration") # Internal USE Variables - Do not modify without consultation self.dynamic_model = utils.ApproximateDynamicsModel() self.ackermann_model = utils.AckermannModel(0.36) self.trajectory = LineTrajectory("/speed_track") self.counts = 0 # Publishers self.viz_track_pub = rospy.Publisher("/speed_track/viz", MarkerArray, queue_size=1) self.traj_pub = rospy.Publisher(rospy.get_param("~pub_topic"), PolygonStamped, queue_size=1) # Subscriber self.traj_sub = rospy.Subscriber(rospy.get_param("~sub_topic"), String, self.trajCB, queue_size=1) print "Initialized. Waiting on messages..." # need to wait a short period of time before publishing the first message time.sleep(0.5) # Callback Function for receiving path def trajCB(self, msg): path_id = msg.data if (self.counts == 1): self.generate_speed_profile(path_id) self.counts = 0 self.counts += 1 # Generating Speed Profile def generate_speed_profile(self, path): self.trajectory.clear() self.trajectory.load(path) self.trajectory.make_np_array() points = self.trajectory.np_points local_polar_points = utils.piecewise_linear_local_waypoints_polar( points) speeds = np.zeros(points.shape[0]) for i in xrange(local_polar_points.shape[0]): max_speed = self.dynamic_model.max_speed_dist( local_polar_points[i, 0]) speeds[i] = max_speed speeds[0] = 0 # speed of the beginning of the track speeds[-1] = 0 # speed of the last of the track speeds = np.clip(speeds, self.min_speed, self.max_speed) # ensure that acceleration bounds are respected for i in xrange(speeds.shape[0] - 1): x = local_polar_points[i, 0] v_i = speeds[i] # initial speed of a straight line v_f = speeds[i + 1] # final speed of a straight line a = (v_f**2 - v_i**2) / (2.0 * x) # vf^2 - vi^2 = 2ax if a > self.max_acceleration: a = self.max_acceleration v_f = np.sqrt(v_i**2 + 2.0 * a * x) speeds[i + 1] = v_f # ensure that decceleration bounds are respected for i in xrange(speeds.shape[0] - 2, -1, -1): x = local_polar_points[i, 0] v_i = speeds[i] v_f = speeds[i + 1] a = (v_f**2 - v_i**2) / (2.0 * x) # vf^2 - vi^2 = 2ax if a < self.max_decceleration: a = self.max_decceleration v_i = np.sqrt(v_f**2 - 2.0 * a * x) speeds[i] = v_i # counting estimated travesal time t_traversal = 0.0 for i in xrange(speeds.shape[0] - 1): v, x = speeds[i], local_polar_points[i, 0] t = x / v t_traversal += t print "Estimated traversal time: ", t_traversal print speeds # Publish the generated Speed Profile self.trajectory.speed_profile = speeds.tolist() self.viz_track_pub.publish( viz_speed_track(self.trajectory.speed_profile, self.trajectory.np_points)) self.traj_pub.publish(self.trajectory.toPolygon())