class LoadTrajectory(object): """ Loads a trajectory from the file system and publishes it to a ROS topic. """ def __init__(self): self.path = rospy.get_param("~trajectory") # initialize and load the trajectory self.trajectory = LineTrajectory("/loaded_trajectory") self.trajectory.load(self.path) self.pub_topic = "/trajectory/current" self.traj_pub = rospy.Publisher(self.pub_topic, PoseArray, queue_size=1) # need to wait a short period of time before publishing the first message rospy.loginfo("waiting to publish trajectory") time.sleep(3.0) # visualize the loaded trajectory self.trajectory.publish_viz() # send the trajectory self.publish_trajectory() def publish_trajectory(self): print "Publishing trajectory to:", self.pub_topic self.traj_pub.publish(self.trajectory.toPoseArray())
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("/map", OccupancyGrid, self.map_cb) self.trajectory = LineTrajectory("/planned_trajectory") self.goal_sub = rospy.Subscriber("/move_base_simple/goal", PoseStamped, self.goal_cb, queue_size=10) self.traj_pub = rospy.Publisher("/trajectory/current", PoseArray, queue_size=10) self.odom_sub = rospy.Subscriber(self.odom_topic, Odometry, self.odom_cb) def map_cb(self, msg): pass ## REMOVE AND FILL IN ## def odom_cb(self, msg): pass ## REMOVE AND FILL IN ## def goal_cb(self, msg): pass ## REMOVE AND FILL IN ## def plan_path(self, start_point, end_point, map): ## CODE FOR PATH PLANNING ## # publish trajectory self.traj_pub.publish(self.trajectory.toPoseArray()) # visualize trajectory Markers self.trajectory.publish_viz()
class BuildTrajectory(object): """ Listens for points published by RViz and uses them to build a trajectory. Saves the output to the file system. """ def __init__(self): self.save_path = os.path.join("trajectories/", time.strftime("%Y-%m-%d-%H-%M-%S") + ".traj") #%Y-%m-%d-%H-%M-%S self.trajectory = LineTrajectory("/built_trajectory") self.data_points = [] self.count = 0 self.click_sub = rospy.Subscriber("/clicked_point", PointStamped, self.clicked_pose, queue_size=10) self.traj_pub = rospy.Publisher("/trajectory/current", PoseArray, queue_size=10) self.trajectory_points = rospy.Publisher("/traj_pts", Marker, queue_size=20) self.trajectory.publish_viz() # save the built trajectory on shutdown rospy.on_shutdown(self.saveTrajectory) def publish_trajectory(self): self.traj_pub.publish(self.trajectory.toPoseArray()) def saveTrajectory(self): self.trajectory.save(self.save_path) def clicked_pose(self, msg): self.count += 1 point = Point() point.x = msg.point.x point.y = msg.point.y self.trajectory.addPoint(point) self.data_points.append(point) self.mark_pt(self.trajectory_points, (0, 1, 0), self.data_points) if self.count > 2: rospy.loginfo("PUBLISH TRAJ") self.publish_trajectory() self.trajectory.publish_viz() def mark_pt(self, subscriber, color_tup, data): mark_pt = Marker() mark_pt.header.frame_id = "/map" mark_pt.header.stamp = rospy.Time.now() mark_pt.type = mark_pt.SPHERE_LIST mark_pt.action = mark_pt.ADD mark_pt.scale.x = .5 mark_pt.scale.y = .5 mark_pt.scale.z = .5 mark_pt.color.a = 1.0 mark_pt.color.r = color_tup[0] mark_pt.color.g = color_tup[1] mark_pt.color.b = color_tup[2] mark_pt.points = data subscriber.publish(mark_pt)
class PathPlan(object): """ Listens for goal pose published by RViz and uses it to plan a path from current car pose. """ def __init__(self): # FIELDS #self.goal_pos = None #self.start_pos = None #self.g_map = None #self.map_x_offset = None #self.map_y_offset = None #self.map_resolution = None # Euler angnles: #self.yaw = 0 #self.pitch = 0 #self.roll = 0 #self.CCW_rotation_matrix = [] #self.CW_rotation_matrix = [] # SUBSCRIBERS and PUBLISHERS #self.odom_topic = rospy.get_param("~odom_topic") self.map_sub = rospy.Subscriber("/map", OccupancyGrid, self.map_cb) self.trajectory = LineTrajectory("/planned_trajectory") self.goal_sub = rospy.Subscriber("/move_base_simple/goal", PoseStamped, self.goal_cb, queue_size=10) self.traj_pub = rospy.Publisher("/trajectory/current", PoseArray, queue_size=10) self.odom_sub = rospy.Subscriber("/tesse/odom", Odometry, self.odom_cb) def map_cb(self, msg): data = np.array(msg.data).reshape((msg.info.height, msg.info.width)) self.map_x_offset = msg.info.origin.position.x self.map_y_offset = msg.info.origin.position.y self.map_resolution = msg.info.resolution # Obtaining the Euler angles and rotation matrix x = msg.info.origin.orientation.x y = msg.info.origin.orientation.y z = msg.info.origin.orientation.z w = msg.info.origin.orientation.w (self.roll, self.pitch, self.yaw) = tf.transformations.euler_from_quaternion([x, y, z, w]) self.CCW_rotation_matrix = np.array( [[np.cos(self.yaw), -np.sin(self.yaw)], [np.sin(self.yaw), np.cos(self.yaw)]]) self.CW_rotation_matrix = np.array( [[np.cos(self.yaw), np.sin(self.yaw)], [-np.sin(self.yaw), np.cos(self.yaw)]]) print("THIS IS THE YAW => " + str(self.yaw)) print("THIS IS THE PITCH => " + str(self.pitch)) print("THIS IS THE ROLL => " + str(self.roll)) # Treat some spots as certain obstacles - May not need this #self.g_map = data self.g_map = np.where(data != 0, 100, data) #self.g_map = np.where(self.g_map > 0, 100, self.g_map) # Dialating the map using scipy kernel = np.array([[1, 1, 1], [1, 1, 1], [1, 1, 1]] * 3) self.g_map = signal.convolve2d(self.g_map, kernel) # VISUALIZING THE MAP - only for debugging plt.imshow(self.g_map) plt.show() def odom_cb(self, msg): x_0 = msg.pose.pose.position.x y_0 = msg.pose.pose.position.y self.start_pos = (x_0, y_0) def goal_cb(self, msg): x_0 = msg.pose.position.x y_0 = msg.pose.position.y self.goal_pos = (x_0, y_0) # Only call the path planned once a goal position has been specified self.plan_path(self.start_pos, self.goal_pos, self.g_map) def plan_path(self, start_position, goal_position, ground_map): rospy.loginfo("Building the path...") # ADJUSTMENT #start x_0 = start_position[0] y_0 = start_position[1] rotated = np.dot(self.CCW_rotation_matrix, np.array([[x_0], [y_0]])) x_r = rotated[0][0] y_r = rotated[1][0] x = (x_r + self.map_x_offset) / self.map_resolution y = (y_r + self.map_y_offset) / self.map_resolution start_d = (x, y) #goal x_0 = goal_position[0] y_0 = goal_position[1] rotated = np.dot(self.CCW_rotation_matrix, np.array([[x_0], [y_0]])) x_r = rotated[0][0] y_r = rotated[1][0] x = (x_r + self.map_x_offset) / self.map_resolution y = (y_r + self.map_y_offset) / self.map_resolution goal_d = (x, y) start = (int(start_d[0]), int(start_d[1])) goal = (int(goal_d[0]), int(goal_d[1])) print("start = " + str(start)) print("goal = " + str(goal)) # Using L-2 norm distance... maybe manhatan would be better? def heuristic(location): return ((location[0] - goal[0])**2 + (location[1] - goal[1])**2)**(1 / 2) # Reconstruct the path using parent pointers def build_path(came_from): path = [] # Fix position path.append(goal_position) position = came_from[goal] while came_from[position] is not None: x_0 = (position[0] * self.map_resolution) - self.map_x_offset y_0 = (position[1] * self.map_resolution) - self.map_y_offset rotated = np.dot(self.CW_rotation_matrix, np.array([[x_0], [y_0]])) x = rotated[0][0] y = rotated[1][0] path.append((x, y)) #path.append(((position[0]*self.map_resolution)+self.map_x_offset,(position[1]*self.map_resolution)+self.map_y_offset)) position = came_from[position] path.append((start_position)) path.reverse() self.trajectory.points = path #rospy.loginfo(str(path)) return # getting the width and height W = len(ground_map[0]) H = len(ground_map) # initializing all my sets #open_queue = Queue.Queue() #open_queue.put(start) open_set = set() open_set.add(start) #closed_set = set() # May not need this came_from = {start: None} g_score = {start: 0} f_start = heuristic(start) f_score = {start: f_start} rev_f_score = {f_start: set()} # just for efficiency rev_f_score[f_start].add(start) all_neighbors = {} found = False # The main part #while not open_queue.empty(): while len(open_set) != 0: #print("Searching...") inter = open_set.intersection(rev_f_score[min(rev_f_score)]) current = inter.pop() #rospy.loginfo("Current = " + str(current)) #current = open_queue.get() if current == goal: build_path(came_from) found = True break open_set.remove(current) rev_f_score[min(rev_f_score)].remove(current) if len(rev_f_score[min(rev_f_score)]) == 0: del rev_f_score[min(rev_f_score)] # Get the neigbnors if they are not already in the `neighbors` dictionary neighbors = [] if current not in all_neighbors: # This just looks bad, but it's OK for now if current[0] + 1 <= W - 1: neighbors.append((current[0] + 1, current[1])) if current[0] - 1 >= 0: neighbors.append((current[0] - 1, current[1])) if current[1] + 1 <= H - 1: neighbors.append((current[0], current[1] + 1)) if current[1] - 1 >= 0: neighbors.append((current[0], current[1] - 1)) #DIAGONALS #Bottom right if current[0] + 1 <= W - 1 and current[1] + 1 <= H - 1: neighbors.append((current[0] + 1, current[1] + 1)) #Bottom left if current[0] + 1 <= W - 1 and current[1] - 1 >= 0: neighbors.append((current[0] + 1, current[1] - 1)) #Top right if current[0] - 1 >= 0 and current[1] + 1 <= H - 1: neighbors.append((current[0] - 1, current[1] + 1)) #Top left if current[0] - 1 >= 0 and current[1] - 1 <= 0: neighbors.append((current[0] - 1, current[1] - 1)) all_neighbors[current] = neighbors[:] else: neighbors = all_neighbors[current][:] for each in neighbors: tentative_score = g_score[current] + 1 # Treating anything with probability higher than 10 as a certain wall if ground_map[each[1]][each[0]] == 0 and ( each not in g_score or tentative_score <= g_score[each]): g_score[each] = tentative_score came_from[each] = current # optimize this part by creating an h = {} and not repeating calculations f_score[each] = g_score[current] + heuristic(each) if f_score[each] not in rev_f_score: rev_f_score[f_score[each]] = set() rev_f_score[f_score[each]].add(each) if each not in open_set: open_set.add(each) #open_queue.put(each) if not found: rospy.loginfo("No Path found!") return # publish trajectory self.traj_pub.publish(self.trajectory.toPoseArray()) # visualize trajectory Markers self.trajectory.publish_viz()
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 PathPlan(object): """ Listens for goal pose published by RViz and uses it to plan a path from current car pose. """ def __init__(self): self.current_pose = None self.goal_pose = None # The map data, in row-major order, starting with (0,0). Occupancy # probabilities are in the range [0,100]. Unknown is -1. self.map_occupancy_prob = None self.obstacles = None # The origin of the map [m, m, rad]. This is the real-world pose of the # cell (0,0) in the map. self.map_origin_pose = None # Map width, height [cells] self.map_dimensions = (0, 0) # The map resolution [m/cell] self.map_res = None # MAKE SURE YOU UNCOMMENT THIS AND SET THE ODOM TOPIC RIGHT # self.odom_topic = rospy.get_param("/particle_filter/odom_topic", "/odom") self.trajectory = LineTrajectory("/planned_trajectory") self.map_sub = rospy.Subscriber("/map", OccupancyGrid, self.map_cb) self.traj_pub = rospy.Publisher("/trajectory/current", PoseArray, queue_size=10) self.goal_sub = rospy.Subscriber("/move_base_simple/goal", PoseStamped, self.goal_cb, queue_size=10) self.odom_sub = rospy.Subscriber("/pf/pose/odom", Odometry, self.odom_cb) def map_cb(self, msg): self.map_origin_pose = msg.info.origin _, _, self.theta = tf.transformations.euler_from_quaternion( (self.map_origin_pose.orientation.x, self.map_origin_pose.orientation.y, self.map_origin_pose.orientation.z, self.map_origin_pose.orientation.w)) self.map_dimensions = (msg.info.width, msg.info.height) map = np.reshape(msg.data, (msg.info.height, msg.info.width)) self.map_occupancy_prob = dilation(map, square(4)) print(self.map_occupancy_prob[339][359]) self.map_res = msg.info.resolution obstacles1 = np.where(self.map_occupancy_prob == 100) obstacles2 = np.where(self.map_occupancy_prob == -1) self.obstacles = set(zip(obstacles1[1], obstacles1[0])).union( set(zip(obstacles2[1], obstacles2[0]))) def odom_cb(self, msg): if self.current_pose is None: print("intialize current pose") #Has .position (Point) and .orientation (Quaternion) attributes self.current_pose = msg.pose.pose pixel_coord = self.map_to_pixel( (self.current_pose.position.x, self.current_pose.position.y)) self.current_pose.position.x = pixel_coord[0] self.current_pose.position.y = pixel_coord[1] def goal_cb(self, msg): print("intialize goal pose") #Has .position (Point) and .orientation (Quaternion) attributes self.goal_pose = msg.pose pixel_coord = self.map_to_pixel( (self.goal_pose.position.x, self.goal_pose.position.y)) self.goal_pose.position.x = pixel_coord[0] self.goal_pose.position.y = pixel_coord[1] while self.current_pose is None or self.goal_pose is None or self.obstacles is None: print('waiting for all the required info') self.plan_path( (self.current_pose.position.x, self.current_pose.position.y), (self.goal_pose.position.x, self.goal_pose.position.y)) # def trace_path(self, node, path = []): # point = Point() # map_coord = self.pixel_to_map((node[0][0], node[0][1])) # point.x = map_coord[0] # point.y = map_coord[1] # path.append(point) # if node[1] != None: # return self.trace_path(node[1], path) # else: # path.reverse() # return path def trace_path(self, start, end, parent): path = [] point, node = Point(), end map_coord = self.pixel_to_map((node[0], node[1])) point.x = map_coord[0] point.y = map_coord[1] path.append(point) while parent[node] != node: node = parent[node] point = Point() map_coord = self.pixel_to_map((node[0], node[1])) point.x = map_coord[0] point.y = map_coord[1] path.append(point) return path[-1::-1] def astar(self, start_position, end_position, obstacles): queue = [] heapq.heappush(queue, (0, start_position)) visited = set() parent = {start_position: start_position} # for path reconstruction mins = {start_position: 0} while queue: _, node = heapq.heappop(queue) if node in visited: continue if node == end_position: return self.trace_path(start_position, end_position, parent) visited.add(node) for neighbor in self.adj_nodes(node): if neighbor in visited or neighbor in obstacles: continue new_cost = mins[node] + self.euclidian_cost(node, neighbor) if neighbor not in mins or mins[neighbor] > new_cost: mins[neighbor] = new_cost priority = new_cost + self.euclidian_cost( neighbor, end_position) heapq.heappush(queue, (priority, neighbor)) parent[neighbor] = node return [] def dijkstra(self, start_position, end_position, obstacles): queue = [] heapq.heappush(queue, (0, start_position)) visited = set() parent = {start_position: start_position} # for path reconstruction mins = {start_position: 0} while queue: _, node = heapq.heappop(queue) if node in visited: continue if node == end_position: return self.trace_path(start_position, end_position, parent) visited.add(node) for neighbor in self.adj_nodes(node): if neighbor in visited or neighbor in obstacles: continue new_cost = mins[node] + self.euclidian_cost(node, neighbor) if neighbor not in mins or mins[neighbor] > new_cost: mins[neighbor] = new_cost heapq.heappush(queue, (new_cost, neighbor)) parent[neighbor] = node return [] # def astar(self, start_position, end_position, obstacles): # print("starting astar") # # queue = [] # heapq.heappush(queue, (0, 0, (start_position, None))) # visited = set() # while queue: # _, cost, path = heapq.heappop(queue) # current = path[0] # if current in visited: # continue # if current == end_position: # return self.trace_path(path, []) # visited.add(current) # # for neighbor in self.adj_nodes(current): # if neighbor in visited or neighbor in obstacles: # continue # # new_cost = cost + self.euclidian_cost(current, neighbor) # priority = new_cost + self.euclidian_cost(neighbor, end_position) # heapq.heappush(queue, (priority, new_cost, (neighbor, path))) # return [] def plan_path(self, start_position, end_position): print('planning path') if end_position in self.obstacles: print("End goal is occupied") return path = self.dijkstra(start_position, end_position, self.obstacles) if not path: print("trajectory not found") self.trajectory.clear() for point in path: self.trajectory.addPoint(point) self.traj_pub.publish(self.trajectory.toPoseArray()) # visualize trajectory Markers self.trajectory.publish_viz() def euclidian_cost(self, position1, position2): #Idk how the dubin curve thing right now so here's a 2D Euclidan heuristic lol return ((position2[0] - position1[0]))**2 + ( (position2[1] - position1[1]))**2 # def dubin_cost(self, pose1, pose2): # x0 = pose1.position.x # y0 = pose1.position.y # _, _, theta0 = tf.transformations.euler_from_quaternion((pose1.orientation.x, pose1.orientation.y, pose1.orientation.z, pose1.orientation.w)) # # x1 = pose2.position.x # y1 = pose2.position.y # _, _, theta1 = tf.transformations.euler_from_quaternion((pose2.orientation.x, pose2.orientation.y, pose2.orientation.z, pose2.orientation.w)) # q0 = (x0, y0, theta0) # q1 = (x1, y1, theta1) # turning_radius = 0.5 # step_size = 0.1 # path = dubins.shortest_path(q0, q1, turning_radius) # configurations, _ = path.sample_many(step_size) def adj_nodes(self, point): #return nodes that are adjacent to point adj_nodes = [] for i in range(-1, 2): for j in range(-1, 2): if 0 <= point[0] + i < self.map_dimensions[ 0] and 0 <= point[1] + j < self.map_dimensions[1]: adj_nodes.append((point[0] + i, point[1] + j)) return adj_nodes def map_to_pixel(self, c): #converts a pose to a pixel shift = np.array([[self.map_origin_pose.position.x], [self.map_origin_pose.position.y]]) c_np = np.array([[c[0]], [c[1]]]) res = self.map_res rotation = np.array([[math.cos(self.theta), math.sin(self.theta)], [-1 * math.sin(self.theta), math.cos(self.theta)]]) p = (np.linalg.inv(rotation).dot((c_np - shift))) / res p = p.flatten() return round(p[0]), round(p[1]) def pixel_to_map(self, c): #converts a pose to a pixel shift = np.array([[self.map_origin_pose.position.x], [self.map_origin_pose.position.y]]) c_np = np.array([[c[0]], [c[1]]]) res = self.map_res rotation = np.array([[math.cos(self.theta), math.sin(self.theta)], [-1 * math.sin(self.theta), math.cos(self.theta)]]) m = res * rotation.dot(c_np) + shift m = m.flatten() return m[0], m[1]