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]
