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 _get_current_waypoint(self, waypoints, lad, position, theta): wpts = waypoints[:, 0:2] nearest_point, nearest_dist, t, i = nearest_point_on_trajectory_py2(position, wpts) if nearest_dist < lad: lookahead_point, i2, t2 = first_point_on_trajectory_intersecting_circle(position, lad, 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