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())
