Drake
pure_pursuit.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include "drake/automotive/gen/pure_pursuit_params.h"
4 #include "drake/automotive/gen/simple_car_params.h"
9 
10 namespace drake {
11 namespace automotive {
12 
13 /// PurePursuit computes the required steering angle to achieve a goal point on
14 /// an continuous planar path. The path represents as the set of `r = 0`
15 /// positions along a Maliput lane, and a goal point is selected as a
16 /// pre-defined lookahead distance along the path in the intended direction of
17 /// travel. The algorithm outputs the steering angle required to guide the
18 /// vehicle toward the goal point based on its current position in global
19 /// coordinates.
20 ///
21 /// See [1] and the corresponding .cc file for details on the algorithm.
22 ///
23 /// Instantiated templates for the following kinds of T's are provided:
24 /// - double
25 /// - AutoDiffXd
26 ///
27 /// They are already available to link against in the containing library.
28 ///
29 /// [1] Coulter, R. Implementation of the Pure Pursuit Path Tracking
30 /// Algorithm. Carnegie Mellon University, Pittsburgh, Pennsylvania, Jan
31 /// 1990.
32 template <typename T>
33 class PurePursuit {
34  public:
36  PurePursuit() = delete;
37 
38  /// Evaluates the required steering angle in radians using the pure-pursuit
39  /// method. Assumes zero elevation and superelevation.
40  ///
41  /// @param pp_params contains the `lookahead_distance`, the distance along the
42  /// path based on the closest position on the path to the vehicle.
43  ///
44  /// @param car_params contains the `wheelbase` of the vehicle.
45  ///
46  /// @param lane_direction is a LaneDirection containing a reference lane and
47  /// the direction of travel along the positive-s coordinate.
48  ///
49  /// @param pose is the PoseVector for the ego vehicle.
50  // TODO(jadecastro): Infer the direction of travel rather than require it.
51  static T Evaluate(const PurePursuitParams<T>& pp_params,
52  const SimpleCarParams<T>& car_params,
53  const LaneDirection& lane_direction,
55 
56  /// Computes the goal point at a distance `s_lookahead` from the closest
57  /// position on the curve in the intended direction of travel, and `with_s`
58  /// and `pose` are the direction of travel and PoseVector for the ego vehicle.
60  const T& s_lookahead, const LaneDirection& lane_direction,
62 };
63 
64 } // namespace automotive
65 } // namespace drake
Definition: automotive_demo.cc:88
A position in 3-dimensional geographical Cartesian space, i.e., in the world frame, consisting of three components x, y, and z.
Definition: lane_data.h:135
static T Evaluate(const PurePursuitParams< T > &pp_params, const SimpleCarParams< T > &car_params, const LaneDirection &lane_direction, const systems::rendering::PoseVector< T > &pose)
Evaluates the required steering angle in radians using the pure-pursuit method.
Definition: pure_pursuit.cc:22
PurePursuit computes the required steering angle to achieve a goal point on an continuous planar path...
Definition: pure_pursuit.h:33
static const maliput::api::GeoPositionT< T > ComputeGoalPoint(const T &s_lookahead, const LaneDirection &lane_direction, const systems::rendering::PoseVector< T > &pose)
Computes the goal point at a distance s_lookahead from the closest position on the curve in the inten...
Definition: pure_pursuit.cc:52
LaneDirection holds the lane that a MaliputRailcar is traversing and the direction in which it is mov...
Definition: lane_direction.h:13
A 7-vector representing the transform of frame A in the world frame, X_WA, in the form {p_WA...
Definition: pose_vector.h:19
Isometry3< double > pose
Definition: pose_smoother.cc:28
#define DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(Classname)
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN deletes the special member functions for copy-construction, copy-assignment, move-construction, and move-assignment.
Definition: drake_copyable.h:33
Provides careful macros to selectively enable or disable the special member functions for copy-constr...