def pure_pursuit(self, pose):
# Stop if no trajectory has been received
if (self.trajectory.empty()):
return self.stop()
# Convert trajectory's information (list of waypoints) into numpy matrix
if (self.trajectory.dirty()):
self.trajectory.make_np_array()
# Step 1
self.nearest_pt, nearest_dist, t, i = utils.nearest_point_on_trajectory(
pose[:2], self.trajectory.np_points)
if (nearest_dist < self.lookahead):
# Step 2
self.lookahead_pt, i2, t2 = utils.first_point_on_trajectory_intersecting_circle(
pose[:2],
self.lookahead,
self.trajectory.np_points,
i + t,
wrap=0)
if (i2 == None):
self.lookahead_pt = None
elif (nearest_dist < self.max_reacquire):
self.lookahead_pt = self.nearest_pt
else:
self.lookahead_pt = None
# Stop vehicle if there is no navigation target
if not isinstance(self.lookahead_pt, np.ndarray):
self.stop()
# Compute driving command based on current pose and lookahead point
else:
xspeed, rotspeed = self.determine_speeds(pose, self.lookahead_pt)
self.apply_control(xspeed, rotspeed)
self.visualize()
def pure_pursuit(self, pose):
''' Determines and applies Pure Pursuit control law
1. Find the nearest point on the trajectory
2. Traverse the trajectory looking for the nearest point that is the
lookahead distance away from the car, and further along the path than
the nearest point from step (1). This is the lookahead point.
3. Determine steering angle necessary to travel to the lookahead point from
step (2)
4. Send the desired speed and steering angle commands to the robot
Special cases:
- If nearest_point is beyond the max path reacquisition distance, stop
- If nearest_point is between max reacquisition dist and lookahead dist,
navigate to nearest_point
- If nearest_point is less than the lookahead distance, find the
lookahead point as normal '''
# stop if no trajectory has been received
if self.trajectory.empty():
return self.stop()
# this instructs the trajectory to convert the list of waypoints into a numpy matrix
if self.trajectory.dirty():
self.trajectory.make_np_array()
# step 1
nearest_point, nearest_dist, t, i = utils.nearest_point_on_trajectory(
pose[:2], self.trajectory.np_points)
self.nearest_point = nearest_point
if nearest_dist < self.lookahead:
# step 2
lookahead_point, i2, t2 = \
utils.first_point_on_trajectory_intersecting_circle(pose[:2], self.lookahead, self.trajectory.np_points, i+t, wrap=self.wrap)
if i2 == None:
if self.iters % 5 == 0:
print "Could not find intersection, end of path?"
self.lookahead_point = None
else:
# if self.iters % 5 == 0:
# print "found lookahead point"
self.lookahead_point = lookahead_point
elif nearest_dist < self.max_reacquire:
if self.iters % 5 == 0:
print "Reacquiring trajectory"
self.lookahead_point = self.nearest_point
else:
self.lookahead_point = None
# stop of there is no navigation target, otherwise use ackermann geometry to navigate there
if not isinstance(self.lookahead_point, np.ndarray):
self.stop()
else:
# Calculate the required driving speeds (linear and rotation)
xspeed, rotspeed = self.determine_speeds(pose,
self.lookahead_point)
# send the control commands
self.apply_control(xspeed, rotspeed)
self.visualize()
def heuristic(self, state, goal_state):
''' Finds the nearest point along the rough trajectory and then uses path length from there to the end as heursitic
'''
nearest_point, nearest_dist, t, i = utils.nearest_point_on_trajectory(
state.center, self.rough_trajectory.np_points)
distance_to_goal_along_trajectory = self.rough_trajectory.distance_to_end(
i + t)
return (distance_to_goal_along_trajectory -
state.radius * 2.0) * self.heuristic_bias
def _get_current_waypoint(self, waypoints, lookahead_distance, position,
theta):
wpts = waypoints[:, 0:2]
nearest_point, nearest_dist, t, i = nearest_point_on_trajectory(
position, wpts)
if nearest_dist < lookahead_distance:
lookahead_point, i2, t2 = first_point_on_trajectory_intersecting_circle(
position, lookahead_distance, wpts, i + t, wrap=True)
if i2 == None:
return None
current_waypoint = np.empty(waypoints[i2, :].shape)
# x, y
current_waypoint[0:2] = waypoints[i2, 0:2]
# theta
current_waypoint[3] = waypoints[i2, 3]
# speed
current_waypoint[2] = waypoints[i2, 2]
return current_waypoint
elif nearest_dist < self.max_reacquire:
return waypoints[i, :]
else:
return None
def measurement(self, pid, pose, pub, best_err=100000.):
''' Calculate the error in path pursuit, given the PID params.
In other words, check the smoothness of the pursuit process.
'''
if self.trajectory.empty():
print 'no trajectory found'
return self.stop()
# print "Start measurement:", pid
# this instructs the trajectory to convert the list of waypoints into a numpy matrix
if self.trajectory.dirty():
self.trajectory.make_np_array()
lookahead_point = np.ndarray(1)
current_pose = deepcopy(pose)
first_angle = True
prev_angle = 0.
int_cte = 0.
diff_cte = 0.
err = 0.
pa = PoseArray()
pa.header = utils.make_header("/map")
step = 0
while isinstance(lookahead_point, np.ndarray) and err < best_err:
pre_pose = current_pose
# step 1
nearest_point, nearest_dist, t, i = utils.nearest_point_on_trajectory(
current_pose[:2], self.trajectory.np_points)
if nearest_dist < self.lookahead:
# step 2
lookahead_point, i2, t2 = \
utils.first_point_on_trajectory_intersecting_circle(current_pose[:2], self.lookahead, self.trajectory.np_points, i+t, wrap=self.wrap)
if i2 == None:
lookahead_point = None
elif nearest_dist < self.max_reacquire:
lookahead_point = nearest_point
else:
lookahead_point = None
if not isinstance(lookahead_point, np.ndarray):
dist = (current_pose[0] - self.trajectory.points[-1][0])**2 + (
current_pose[1] - self.trajectory.points[-1][1])**2
if dist > self.max_reacquire**2:
err += best_err + 1
# print "Cannot get the end, stopping Car"
else:
# print "move_point: ", current_pose
steering_angle = self.determine_steering_angle(
current_pose, lookahead_point)
if first_angle:
prev_angle = steering_angle
int_cte = 0.
first_angle = False
diff_cte = steering_angle - prev_angle
pid_angle = pid[0] * steering_angle + pid[2] * diff_cte + pid[
1] * int_cte
if np.abs(pid_angle) > self.max_angle:
pid_angle = self.max_angle * np.sign(pid_angle)
# TODO: Wrong!
# err += np.abs(pid_angle - prev_angle)
err += nearest_dist + np.abs(prev_angle - pid_angle)
# print "err", err, "; angle", pid_angle, "; current_pose", current_pose
int_cte += pid_angle
prev_angle = pid_angle
current_pose = self.move(current_pose,
self.speed,
pid_angle,
duration=1.0)
dist = (current_pose[0] - pre_pose[0])**2 + (current_pose[1] -
pre_pose[1])**2
if dist < 0.001:
err += (self.lookahead + 1) / (dist + 0.1)
if self.isCollision(current_pose, self.map_data, self.margin):
# print "collision!"
err += best_err + 1
if self.viz:
tmp = Pose()
tmp.position.x = current_pose[0]
tmp.position.y = current_pose[1]
tmp.position.z = 0.
tmp.orientation = utils.angle_to_quaternion(
current_pose[2])
pa.poses.append(tmp)
# if step % 30 == 0:
# pub.publish(pa)
if self.viz and err <= best_err:
print 'Better pid params found.'
pub.publish(pa)
time.sleep(0.1)
# print "End measurement: err", err
return err
