Drake
pure_pursuit_controller.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <memory>
4 
5 #include <Eigen/Geometry>
6 
7 #include "drake/automotive/gen/pure_pursuit_params.h"
8 #include "drake/automotive/gen/simple_car_params.h"
13 
14 namespace drake {
15 namespace automotive {
16 
17 /// PurePursuitController implements a pure pursuit controller. See PurePursuit
18 /// for details on the approach.
19 ///
20 /// Instantiated templates for the following kinds of T's are provided:
21 /// - double
22 /// - AutoDiffXd
23 ///
24 /// They are already available to link against in the containing library.
25 ///
26 /// Input Port 0: a LaneDirection representing the requested lane and direction
27 /// of travel.
28 /// (InputPortDescriptor getter: lane_input())
29 ///
30 /// Input Port 1: PoseVector for the ego car.
31 /// (InputPortDescriptor getter: ego_pose_input())
32 ///
33 /// Output Port 0: A BasicVector of size one with the commanded steering angle.
34 /// (OutputPort getter: steering_command_output())
35 ///
36 /// @ingroup automotive_controllers
37 template <typename T>
39  public:
41 
42  /// Constructor.
44 
45  /// Scalar-converting copy constructor. See @ref system_scalar_conversion.
46  template <typename U>
48  : PurePursuitController<T>() {}
49 
50  ~PurePursuitController() override;
51 
52  /// Returns the port to the individual input/output ports.
56 
57  private:
58  void OutputSteeringCommand(const systems::Context<T>& context,
59  systems::BasicVector<T>* output) const;
60 
61  void CalcSteeringCommand(const PurePursuitParams<T>& pp_params,
62  const SimpleCarParams<T>& car_params,
63  const LaneDirection& lane_direction,
64  const systems::rendering::PoseVector<T>& ego_pose,
65  systems::BasicVector<T>* command) const;
66 
67  // Indices for the input / output ports.
68  const int lane_index_{};
69  const int ego_pose_index_{};
70  const int steering_command_index_{};
71 };
72 
73 } // namespace automotive
74 
75 namespace systems {
76 namespace scalar_conversion {
77 // Disables symbolic support, because maliput's LanePositionT <-> GeoPositionT
78 // conversion (used in pure_pursuit.cc) is not symbolic-supported.
79 template <>
80 struct Traits<automotive::PurePursuitController> : public NonSymbolicTraits {};
81 } // namespace scalar_conversion
82 } // namespace systems
83 
84 } // namespace drake
Definition: automotive_demo.cc:88
Context is an abstract base class template that represents all the inputs to a System: time...
Definition: query_handle.h:10
InputPortDescriptor is a notation for specifying the kind of input a System accepts, on a given port.
Definition: input_port_descriptor.h:21
An OutputPort belongs to a System and represents the properties of one of that System&#39;s output ports...
Definition: output_port.h:67
PurePursuitController implements a pure pursuit controller.
Definition: pure_pursuit_controller.h:38
~PurePursuitController() override
Definition: pure_pursuit_controller.cc:37
A superclass template that extends System with some convenience utilities that are not applicable to ...
Definition: leaf_system.h:82
LaneDirection holds the lane that a MaliputRailcar is traversing and the direction in which it is mov...
Definition: lane_direction.h:13
PurePursuitController(const PurePursuitController< U > &)
Scalar-converting copy constructor. See System Scalar Conversion.
Definition: pure_pursuit_controller.h:47
A templated traits class for whether an S<T> can be converted into an S<U>; the default value is true...
Definition: scalar_conversion_traits.h:30
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
BasicVector is a semantics-free wrapper around an Eigen vector that satisfies VectorBase.
Definition: basic_vector.h:25
A concrete traits class providing sugar to disable support for symbolic evaluation (i...
Definition: scalar_conversion_traits.h:52
const systems::InputPortDescriptor< T > & lane_input() const
Returns the port to the individual input/output ports.
Definition: pure_pursuit_controller.cc:40
const systems::OutputPort< T > & steering_command_output() const
Definition: pure_pursuit_controller.cc:53
const systems::InputPortDescriptor< T > & ego_pose_input() const
Definition: pure_pursuit_controller.cc:47
#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
PurePursuitController()
Constructor.
Definition: pure_pursuit_controller.cc:20
Provides careful macros to selectively enable or disable the special member functions for copy-constr...